matrix: fix slice to slice assignment to do deep copy

src/lib/matrix/matrix/Slice.hpp

@@ -35,6 +35,8 @@ class Slice
 		assert(y0 + Q <= N);
 	}
 
+	Slice(const Slice<Type, P, Q, M, N> &other) = default;
+
 	const Type &operator()(size_t i, size_t j) const
 	{
 		assert(i < P);
@@ -52,6 +54,12 @@ class Slice
 		return (*_data)(_x0 + i, _y0 + j);
 	}
 
+	// Separate function needed otherwise the default copy constructor matches before the deep copy implementation
+	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, M, N> &other)
+	{
+		return this->operator=<M, N>(other);
+	}
+
 	template<size_t MM, size_t NN>
 	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
 	{

src/lib/matrix/test/MatrixSliceTest.cpp

@@ -262,3 +262,12 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
+
+TEST(MatrixSliceTest, XYAssignmentTest)
+{
+	Vector3f a(1, 2, 3);
+	Vector3f b(4, 5, 6);
+	// Assign first two elements from b to first two slot of a
+	a.xy() = b.xy();
+	EXPECT_EQ(a, Vector3f(4, 5, 3));
+}

src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv

@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 90000,1,-0.0096,-0.01,-0.02,-0.0004,0.0026,-0.026,-1.4e-05,0.00011,-0.0023,0,0,0,0,0,0,0.19,-4.7e-10,0.4,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 190000,1,-0.0096,-0.011,-0.02,0.0002,0.0039,-0.041,6e-06,0.00043,-0.0044,-3e-13,-2.3e-14,5.8e-15,0,0,-2e-11,0.19,-4.7e-10,0.4,0,0,0,0,0,5.8e-07,0.0027,0.0027,0.0008,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 290000,1,-0.0097,-0.011,-0.02,0.0012,0.0063,-0.053,5e-05,0.00036,-0.007,9.1e-12,9.1e-13,-1.7e-13,0,0,-4.8e-09,0.19,-4.7e-10,0.4,0,0,0,0,0,6.8e-07,0.0029,0.0029,0.00067,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
@@ -132,7 +132,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 12990000,0.7,0.0012,-0.014,0.71,-0.0078,0.0067,-0.03,-0.0011,0.0013,-3.7e+02,-1.2e-05,-5.9e-05,-2.3e-06,6.8e-06,9.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.062,0.062,0.025,0.049,0.049,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0
 13090000,0.7,0.0012,-0.014,0.71,-0.0084,0.0069,-0.03,-0.0019,0.002,-3.7e+02,-1.2e-05,-5.8e-05,-2.8e-06,6.4e-06,9.8e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00021,0.00021,9.3e-05,0.071,0.071,0.025,0.057,0.057,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0
 13190000,0.7,0.00095,-0.014,0.71,0.00044,0.0063,-0.027,0.0043,0.0013,-3.7e+02,-1.1e-05,-5.9e-05,-2.8e-06,5.1e-06,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.058,0.058,0.025,0.049,0.049,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
-13290000,0.7,0.00095,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
+13290000,0.7,0.00096,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0
 13390000,0.7,0.00081,-0.014,0.71,0.001,0.0062,-0.02,0.0032,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-2e-06,2.6e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.055,0.055,0.026,0.048,0.048,0.05,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,8.8e-07,0,0,0,0,0,0,0,0
 13490000,0.7,0.00084,-0.014,0.71,0.0006,0.0062,-0.019,0.0033,0.0018,-3.7e+02,-1.1e-05,-5.9e-05,-1.7e-06,1.8e-06,1.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.063,0.063,0.028,0.056,0.056,0.05,3.5e-09,3.5e-09,1.8e-09,3.5e-06,3.5e-06,8.7e-07,0,0,0,0,0,0,0,0
 13590000,0.7,0.00078,-0.014,0.71,0.0008,0.0064,-0.021,0.0023,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-1.9e-06,1.7e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.052,0.052,0.028,0.048,0.048,0.05,3.3e-09,3.4e-09,1.8e-09,3.5e-06,3.5e-06,8.4e-07,0,0,0,0,0,0,0,0

src/modules/ekf2/test/change_indication/iris_gps.csv

@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
+10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 90000,0.98,-0.0095,-0.012,0.18,-5.5e-05,-0.0032,-0.024,-3.6e-06,-0.00014,-0.0021,0,0,0,0,0,0,0.2,-3.3e-09,0.43,0,0,0,0,0,8.9e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 190000,0.98,-0.0092,-0.013,0.21,-0.0013,-0.0036,-0.037,-4.4e-05,-0.00046,-0.017,5.2e-12,-4.3e-12,-1.5e-13,0,0,-6.8e-10,0.2,0.011,0.43,0,0,0,0,0,2.8e-06,0.0027,0.0027,0.00081,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
 290000,0.98,-0.0092,-0.013,0.21,-0.0016,-0.0053,-0.046,-0.00015,-0.00032,-0.018,4.4e-11,-5.4e-11,-2.6e-12,0,0,-2.9e-08,0.2,0.011,0.43,0,0,0,0,0,6.6e-06,0.0029,0.0029,0.00068,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0
@@ -347,5 +347,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 34490000,0.98,-0.0069,-0.013,0.18,-0.016,-0.0096,-0.09,0.071,-0.011,-0.11,-1.4e-05,-5.6e-05,2.3e-06,0.00017,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.3e-05,3.3e-05,4.2e-05,0.053,0.053,0.0059,0.051,0.051,0.032,2.6e-11,2.6e-11,8.1e-11,2.3e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0
 34590000,0.98,-0.0069,-0.013,0.18,-0.014,-0.0053,0.71,0.073,-0.0089,-0.081,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3e-05,4.2e-05,0.044,0.044,0.0059,0.045,0.045,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0
 34690000,0.98,-0.0068,-0.012,0.18,-0.017,-0.0032,1.7,0.071,-0.0093,0.037,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3.1e-05,4.2e-05,0.047,0.047,0.006,0.052,0.052,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0
-34790000,0.98,-0.0068,-0.012,0.18,-0.018,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
+34790000,0.98,-0.0068,-0.012,0.18,-0.019,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0
 34890000,0.98,-0.0068,-0.012,0.18,-0.022,0.0039,3.6,0.07,-0.0065,0.5,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.043,0.043,0.0061,0.052,0.052,0.032,2.6e-11,2.6e-11,7.8e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0