[kalman] State vector x now initialized in the config struct

Luca avevi ragione, ha decisamente più senso

src/shared/kalman/ExtendedKalmanEigen.cpp

@@ -24,16 +24,13 @@
 
 ExtendedKalmanEigen::ExtendedKalmanEigen(const ExtendedKalmanConfig& config)
     : n(config.n), m(config.m), p(config.p), F(config.F), H(config.H),
-      Q(config.Q), R(config.R), P(config.P), I(config.n, config.n), x(config.n),
+      Q(config.Q), R(config.R), P(config.P), I(config.n, config.n), x(config.x),
       y_hat(config.p), res(config.p), f(config.f), dfdx(config.dfdx),
       h(config.h), dhdx(config.dhdx)
 {
-    x.setZero();
     I.setIdentity();
 }
 
-void ExtendedKalmanEigen::init(const VectorXf& x0) { x = x0; }
-
 void ExtendedKalmanEigen::predict(const VectorXf& u)
 {
     F = dfdx(x, u);  // update jacobian
@@ -51,8 +48,10 @@ bool ExtendedKalmanEigen::correct(const VectorXf& y, const function_v& h,
                                   const function_v& dhdx)
 {
     p = y.rows();
-    S.resize(p, p);
-    K.resize(n, p);
+    MatrixXf S(p, p);
+    MatrixXf K(n, p);
+    //Matrix<float, p, p> S;
+    //Matrix<float, n, p> K;
 
     H = dhdx(x);  // update jacobian
 
@@ -61,7 +60,7 @@ bool ExtendedKalmanEigen::correct(const VectorXf& y, const function_v& h,
     // here the determinant is computed and
     // then the inverse recomputes it,
     // this passage could be optimized
-    if (S.determinant() < 1e-3)
+    if (S.determinant() < 1e-5)
     {
         return false;
     }

src/shared/kalman/ExtendedKalmanEigen.h

@@ -34,8 +34,6 @@ class ExtendedKalmanEigen
 public:
     ExtendedKalmanEigen(const ExtendedKalmanConfig& config);
 
-    void init(const VectorXf& x0);
-
     void predict(const VectorXf& u);
 
     /**
@@ -77,7 +75,7 @@ public:
 private:
     uint8_t n, m, p; /**< system dimensions */
 
-    MatrixXf F, H, Q, R, P, S, K;
+    MatrixXf F, H, Q, R, P;
 
     MatrixXf I; /**< (n x n) identity matrix */
     VectorXf x, y_hat, res;

src/shared/kalman/KalmanConfig.h

@@ -34,6 +34,7 @@ struct KalmanConfig
     MatrixXf Q;
     MatrixXf R;
     MatrixXf P;
+    VectorXf x;
     bool with_ex_input = false; // Set to true is G is assigned (an exogenous input is present)
 };
 
@@ -53,6 +54,7 @@ struct ExtendedKalmanConfig
     MatrixXf Q;
     MatrixXf R;
     MatrixXf P;
+    VectorXf x;
 
     function_2v f;
     function_v h;