matrix: add explicit Vector4 type

src/lib/matrix/matrix/Vector4.hpp

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector4.hpp
+ *
+ * 4D vector class.
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M, size_t N>
+class Matrix;
+
+template <typename Type, size_t M>
+class Vector;
+
+template<typename Type>
+class Vector4 : public Vector<Type, 4>
+{
+public:
+	using Matrix41 = Matrix<Type, 4, 1>;
+
+	Vector4() = default;
+
+	Vector4(const Matrix41 &other) :
+		Vector<Type, 4>(other)
+	{
+	}
+
+	explicit Vector4(const Type data_[3]) :
+		Vector<Type, 4>(data_)
+	{
+	}
+
+	Vector4(Type x1, Type x2, Type x3, Type x4)
+	{
+		Vector4 &v(*this);
+		v(0) = x1;
+		v(1) = x2;
+		v(2) = x3;
+		v(3) = x4;
+	}
+
+	template<size_t P, size_t Q>
+	Vector4(const Slice<Type, 4, 1, P, Q> &slice_in) : Vector<Type, 4>(slice_in)
+	{
+	}
+
+	template<size_t P, size_t Q>
+	Vector4(const Slice<Type, 1, 4, P, Q> &slice_in) : Vector<Type, 4>(slice_in)
+	{
+	}
+};
+
+using Vector4f = Vector4<float>;
+
+} // namespace matrix

src/lib/matrix/matrix/math.hpp

@@ -11,6 +11,7 @@
 #include "Vector.hpp"
 #include "Vector2.hpp"
 #include "Vector3.hpp"
+#include "Vector4.hpp"
 #include "Euler.hpp"
 #include "Dcm.hpp"
 #include "Scalar.hpp"

src/lib/matrix/test/MatrixVector4Test.cpp

@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+TEST(MatrixVector4Test, Vector4)
+{
+	Vector4f a(1.f, 2.f, 3.f, 4.f);
+	EXPECT_EQ(a(0), 1.f);
+	EXPECT_EQ(a(1), 2.f);
+	EXPECT_EQ(a(2), 3.f);
+	EXPECT_EQ(a(3), 4.f);
+}

src/modules/ekf2/EKF/ekf.h

@@ -64,7 +64,6 @@ public:
 	typedef matrix::Vector<float, _k_num_states> Vector24f;
 	typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
 	typedef matrix::SquareMatrix<float, 2> Matrix2f;
-	typedef matrix::Vector<float, 4> Vector4f;
 	template<int ... Idxs>
 
 	using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;