Update ConstVelKF1.cpp

Just changed the shift on the A matrix
StanfordASL · Dec 29, 2023 · 641e09b · 641e09b
1 parent 8af65f2
commit 641e09b
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/ff_estimate/src/ConstVelKF1.cpp b/ff_estimate/src/ConstVelKF1.cpp
@@ -127,7 +127,7 @@ class ConstVelKF {
     std::array<std::array<float, 6>, 6> A;
     std::array<float, 2> dtmatrix = {dt, dt};
     A = diagonal_matrix_offset<6,6>(diagonalone, 0);
-    A = diagonal_matrix_offset<6,6>(dtmatrix, 2);
+    A = diagonal_matrix_offset<6,6>(dtmatrix, 4);
 
     this->x = matrix_multiply(A, this->x);
     this->P = add_matrix(matrix_multiply(matrix_multiply(A, this->P), transpose(A)), matrix_const_mult(Q, dt));