From 641e09b3be41581c9c2d212f5af7cce78485fe13 Mon Sep 17 00:00:00 2001
From: SunFireKing <65515101+SunFireKing@users.noreply.github.com>
Date: Fri, 29 Dec 2023 18:30:44 -0500
Subject: [PATCH] Update ConstVelKF1.cpp

Just changed the shift on the A matrix
---
 ff_estimate/src/ConstVelKF1.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ff_estimate/src/ConstVelKF1.cpp b/ff_estimate/src/ConstVelKF1.cpp
index a58e7f2..8ac9a8d 100644
--- 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));