Bias constants

Author: Hlpred

include/MissionConstants.hpp

@@ -88,7 +88,7 @@ namespace MissionConstants
     const Eigen::Vector3d kSensorCameraPosition = Eigen::Vector3d(0.0, 0.0, 0.0);                                                // Position of the camera in the body frame (in meters)
     const Eigen::Vector3d kSensorCameraOrientationRad = Eigen::Vector3d(3.141592653589793 / 2.0, -3.141592653589793 / 2.0, 0.0); // XYZ Euler orientation from camera frame to body frame
     const Eigen::Vector3d kSensorMagnetometerPosition = Eigen::Vector3d(0.0, 0.0, 0.0);                                          // Position of magnetometer in the body frame (in meters)
-    const Eigen::Vector3d kSensorImuAccelBiasSensorMps2 = Eigen::Vector3d(0.0, 0.0, 0.0);                                         // Accelerometer bias correction in IMU sensor axes
+    const Eigen::Vector3d kSensorImuAccelBiasSensorMps2 = Eigen::Vector3d(-0.44, 0.43, -1.82);                                         // Accelerometer bias correction in IMU sensor axes
     // IMU axis remap from IMU sensor frame to vehicle body frame.
     // Must remain a right-angle transform: each row/column has exactly one +/-1 and zeros elsewhere.
     // Rows are body X/Y/Z, columns are sensor X/Y/Z.

src/Navigation.cpp

@@ -394,7 +394,7 @@ void Navigation::UpdateNavigation()
     // Update state estimates with available measurements
 
     // Debug pseudo-measurement: softly anchor horizontal position to launch-frame origin.
-    debugZeroXYPositionUpdate();
+    // debugZeroXYPositionUpdate();
 
     // Update magnetometer on fixed cadence
     ++magnetometer_update_counter;
@@ -548,15 +548,15 @@ void Navigation::gpsVelocityUpdate(const Eigen::Vector2d &gpsVelocity)
 
 void Navigation::debugZeroXYPositionUpdate()
 {
-    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 15);
-    H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
+    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(2, 15);
+    H.block<2, 2>(0, 0) = Eigen::Matrix2d::Identity();
 
-    Eigen::VectorXd y = Eigen::VectorXd::Zero(3);
-    Eigen::VectorXd y_pred = Eigen::VectorXd::Zero(3);
-    y_pred << x_e(0), x_e(1), x_e(2);
+    Eigen::VectorXd y = Eigen::VectorXd::Zero(2);
+    Eigen::VectorXd y_pred = Eigen::VectorXd::Zero(2);
+    y_pred << x_e(0), x_e(1);
 
     const double variance = MissionConstants::kNavDebugXYZeroPositionVariance;
-    Eigen::MatrixXd V = variance * Eigen::Matrix3d::Identity();
+    Eigen::MatrixXd V = variance * Eigen::Matrix2d::Identity();
 
     kalmanUpdate(H, V, y, y_pred);
 }