flight and ground estimator now pull in gps covariance matrices

Author: jacobh460

lib/controller/FlightEstimator.cpp

@@ -1,5 +1,7 @@
 #include "matlab_funcs.h"
 
+#include "GPS.h"
+
 #define RTK 1
 
 Matrix9_9 FlightStateTransitionMat(Vector3 accel, Vector3 gyro, Matrix3_3 R_b2i) {
@@ -58,10 +60,15 @@ Vector19 FlightEstimator(Vector19 x_est, constantsASTRA_t constantsASTRA, Vector
     H.block<3, 3>(0, 3) = Matrix3_3::Identity();
     H.block<3, 3>(3, 6) = Matrix3_3::Identity();
 
-    // Measurement Covariance Matrix
-    float gps_pos_covar = 0.2 * RTK + 10 * (1 - RTK);
-    float gps_vel_covar = 0.75;
-    Matrix6_6 R = (Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished().asDiagonal();
+    Matrix3_3 gps_pos_covar;
+    Matrix3_3 gps_vel_covar;
+
+    GPS::get_pos_cov(gps_pos_covar);
+    GPS::get_vel_cov(gps_vel_covar);
+
+    Matrix6_6 R = Matrix6_6::Zero();
+    R.block<3, 3>(0, 0) = gps_pos_covar;
+    R.block<3, 3>(3, 3) = gps_vel_covar;
 
     // A priori covariance and Kalman gain
     Matrix9_6 L = P * H.transpose() * (H * P * H.transpose() + R).inverse();

lib/controller/GroundEstimator.cpp

@@ -1,5 +1,7 @@
 #include "matlab_funcs.h"
 
+#include "GPS.h"
+
 #define RTK 1
 
 Matrix18_18 GroundStateTransitionMat(Vector3 accel, Vector3 gyro, Matrix3_3 R_b2i) {
@@ -83,10 +85,15 @@ Vector19 GroundEstimator(Vector19 x_est, constantsASTRA_t constantsASTRA, Vector
     H.block<3, 3>(0, 3) = Matrix3_3::Identity();
     H.block<3, 3>(3, 6) = Matrix3_3::Identity();
 
-    // Measurement Covariance Matrix
-    float gps_pos_covar = 0.5 * RTK + 10 * (1 - RTK);
-    float gps_vel_covar = 0.5;
-    R = (Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished().asDiagonal();
+    Matrix3_3 gps_pos_covar;
+    Matrix3_3 gps_vel_covar;
+
+    GPS::get_pos_cov(gps_pos_covar);
+    GPS::get_vel_cov(gps_vel_covar);
+
+    Matrix6_6 R = Matrix6_6::Zero();
+    R.block<3, 3>(0, 0) = gps_pos_covar;
+    R.block<3, 3>(3, 3) = gps_vel_covar;
 
     // A priori covariance and Kalman gain
     Matrix18_6 L = P * H.transpose() * (H * P * H.transpose() + R).inverse();