added debug launch file, added state augmentation, added jakobi concat; resulting jakobis do not pass gating test

include/msckf_vio/feature.hpp

@@ -496,7 +496,7 @@ bool Feature::initializeAnchor(
 
   //initialize patch Size
   //TODO make N size a ros parameter
-  int N = 3;
+  int N = 13;
   int n = (int)(N-1)/2;
 
   auto anchor = observations.begin();

include/msckf_vio/msckf_vio.h

@@ -162,6 +162,7 @@ class MsckfVio {
 
     // Measurement update
     void stateAugmentation(const double& time);
+    void PhotometricStateAugmentation(const double& time);
     void addFeatureObservations(const CameraMeasurementConstPtr& msg);
     // This function is used to compute the measurement Jacobian
     // for a single feature observed at a single camera frame.
@@ -180,9 +181,9 @@ class MsckfVio {
     void PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
-    Eigen::Matrix<double, 4, 6>& H_x,
+    Eigen::MatrixXd& H_x,
-    Eigen::Matrix<double, 4, 3>& H_f,
+    Eigen::MatrixXd& H_y,
-    Eigen::Vector4d& r);
+    Eigen::VectorXd& r);
 
     void PhotometricFeatureJacobian(
     const FeatureIDType& feature_id,
@@ -224,6 +225,8 @@ class MsckfVio {
     CameraCalibration cam0;
     CameraCalibration cam1;
 
+    // covariance data
+    double irradiance_frame_bias;
 
 
     ros::Time image_save_time;

launch/msckf_vio_debug_tum.launch

@@ -0,0 +1,66 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="fixed_frame_id" default="world"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <include file="$(find msckf_vio)/launch/image_processor_tum.launch">
+    <arg name="robot" value="$(arg robot)"/>
+    <arg name="calibration_file" value="$(arg calibration_file)"/>
+  </include>
+
+  <!-- Msckf Vio Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="vio"
+      args='standalone msckf_vio/MsckfVioNodelet'
+      output="screen"
+      launch-prefix="xterm -e gdb --args">
+
+      <!-- Calibration parameters -->
+      <rosparam command="load" file="$(arg calibration_file)"/>
+
+      <param name="publish_tf" value="true"/>
+      <param name="frame_rate" value="20"/>
+      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
+      <param name="child_frame_id" value="odom"/>
+      <param name="max_cam_state_size" value="20"/>
+      <param name="position_std_threshold" value="8.0"/>
+
+      <param name="rotation_threshold" value="0.2618"/>
+      <param name="translation_threshold" value="0.4"/>
+      <param name="tracking_rate_threshold" value="0.5"/>
+
+      <!-- Feature optimization config -->
+      <param name="feature/config/translation_threshold" value="-1.0"/>
+
+      <!-- These values should be standard deviation -->
+      <param name="noise/gyro" value="0.005"/>
+      <param name="noise/acc" value="0.05"/>
+      <param name="noise/gyro_bias" value="0.001"/>
+      <param name="noise/acc_bias" value="0.01"/>
+      <param name="noise/feature" value="0.035"/>
+
+      <param name="initial_state/velocity/x" value="0.0"/>
+      <param name="initial_state/velocity/y" value="0.0"/>
+      <param name="initial_state/velocity/z" value="0.0"/>
+
+      <!-- These values should be covariance -->
+      <param name="initial_covariance/velocity" value="0.25"/>
+      <param name="initial_covariance/gyro_bias" value="0.01"/>
+      <param name="initial_covariance/acc_bias" value="0.01"/>
+      <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
+      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
+      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
+      <remap from="~features" to="image_processor/features"/>
+
+    </node>
+  </group>
+
+</launch>

launch/msckf_vio_tum.launch

@@ -51,6 +51,7 @@
       <param name="initial_covariance/acc_bias" value="0.01"/>
       <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
       <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
+      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
 
       <remap from="~imu" to="/imu0"/>
       <remap from="~cam0_image" to="/cam0/image_raw"/>

src/msckf_vio.cpp

@@ -117,6 +117,10 @@ bool MsckfVio::loadParameters() {
   nh.param<double>("initial_covariance/extrinsic_translation_cov",
       extrinsic_translation_cov, 1e-4);
 
+  // Get the initial irradiance covariance
+  nh.param<double>("initial_covariance/irradiance_frame_bias",
+      irradiance_frame_bias, 0.1);
+
   // get camera information (used for back projection)
   nh.param<string>("cam0/distortion_model",
       cam0.distortion_model, string("radtan"));
@@ -336,7 +340,7 @@ void MsckfVio::imageCallback(
 
   // Augment the state vector.
   start_time = ros::Time::now();
-  stateAugmentation(feature_msg->header.stamp.toSec());
+  PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
   double state_augmentation_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -511,6 +515,10 @@ bool MsckfVio::resetCallback(
   nh.param<double>("initial_covariance/extrinsic_translation_cov",
       extrinsic_translation_cov, 1e-4);
 
+  // Reset the irradiance covariance
+  nh.param<double>("initial_covariance/irradiance_frame_bias",
+      irradiance_frame_bias, 0.1);
+
   state_server.state_cov = MatrixXd::Zero(21, 21);
   for (int i = 3; i < 6; ++i)
     state_server.state_cov(i, i) = gyro_bias_cov;
@@ -820,6 +828,8 @@ void MsckfVio::stateAugmentation(const double& time) {
   // Resize the state covariance matrix.
   size_t old_rows = state_server.state_cov.rows();
   size_t old_cols = state_server.state_cov.cols();
+
+  // add 7 for camera state + irradiance bias eta = b_l
   state_server.state_cov.conservativeResize(old_rows+6, old_cols+6);
 
   // Rename some matrix blocks for convenience.
@@ -839,10 +849,78 @@ void MsckfVio::stateAugmentation(const double& time) {
   MatrixXd state_cov_fixed = (state_server.state_cov +
       state_server.state_cov.transpose()) / 2.0;
   state_server.state_cov = state_cov_fixed;
-
   return;
 }
 
+void MsckfVio::PhotometricStateAugmentation(const double& time) {
+
+  const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0;
+  const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu;
+
+  // Add a new camera state to the state server.
+  Matrix3d R_w_i = quaternionToRotation(
+      state_server.imu_state.orientation);
+  Matrix3d R_w_c = R_i_c * R_w_i;
+  Vector3d t_c_w = state_server.imu_state.position +
+    R_w_i.transpose()*t_c_i;
+
+  state_server.cam_states[state_server.imu_state.id] =
+    CAMState(state_server.imu_state.id);
+  CAMState& cam_state = state_server.cam_states[
+    state_server.imu_state.id];
+
+  cam_state.time = time;
+  cam_state.orientation = rotationToQuaternion(R_w_c);
+  cam_state.position = t_c_w;
+
+  cam_state.orientation_null = cam_state.orientation;
+  cam_state.position_null = cam_state.position;
+
+
+  // Update the covariance matrix of the state.
+  // To simplify computation, the matrix J below is the nontrivial block
+  // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision
+  // -aided Inertial Navigation".
+  Matrix<double, 6, 21> J = Matrix<double, 6, 21>::Zero();
+  J.block<3, 3>(0, 0) = R_i_c;
+  J.block<3, 3>(0, 15) = Matrix3d::Identity();
+  J.block<3, 3>(3, 0) = skewSymmetric(R_w_i.transpose()*t_c_i);
+  //J.block<3, 3>(3, 0) = -R_w_i.transpose()*skewSymmetric(t_c_i);
+  J.block<3, 3>(3, 12) = Matrix3d::Identity();
+  J.block<3, 3>(3, 18) = Matrix3d::Identity();
+
+  // Resize the state covariance matrix.
+  size_t old_rows = state_server.state_cov.rows();
+  size_t old_cols = state_server.state_cov.cols();
+
+  // add 7 for camera state + irradiance bias eta = b_l
+  state_server.state_cov.conservativeResize(old_rows+7, old_cols+7);
+
+  // Rename some matrix blocks for convenience.
+  const Matrix<double, 21, 21>& P11 =
+    state_server.state_cov.block<21, 21>(0, 0);
+  const MatrixXd& P12 =
+    state_server.state_cov.block(0, 21, 21, old_cols-21);
+
+  // Fill in the augmented state covariance.
+  state_server.state_cov.block(old_rows, 0, 6, old_cols) << J*P11, J*P12;
+  state_server.state_cov.block(0, old_cols, old_rows, 6) =
+    state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose();
+  state_server.state_cov.block<6, 6>(old_rows, old_cols) =
+    J * P11 * J.transpose();
+
+  // Add photometry P_eta and surrounding Zeros
+  state_server.state_cov.block<1, 12>(old_rows+6, 0) = Matrix<double, 1, 12>::Zero();
+  state_server.state_cov.block<12, 1>(0, old_cols+6) = Matrix<double, 12, 1>::Zero();
+  state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias;
+  // Fix the covariance to be symmetric
+  MatrixXd state_cov_fixed = (state_server.state_cov +
+      state_server.state_cov.transpose()) / 2.0;
+  state_server.state_cov = state_cov_fixed;
+  return;
+}
+
+
 
 void MsckfVio::addFeatureObservations(
     const CameraMeasurementConstPtr& msg) {
@@ -879,7 +957,7 @@ void MsckfVio::addFeatureObservations(
 void MsckfVio::PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
-    Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {
+    MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) {
 
   // Prepare all the required data.
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
@@ -890,7 +968,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
   const Vector3d& t_c0_w = cam_state.position;
 
   //temp N
-  const int N = 3;
+  const int N = 13;
 
   // Cam1 pose.
   Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
@@ -940,7 +1018,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
     dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
     dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame)- feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
     dI_dhj(0, 0) = dx;
-    dI_dhj(1, 0) = dy;
+    dI_dhj(0, 1) = dy;
     
     //dh / d{}^Cp_{ij}
     dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix<double, 2, 2>::Identity();
@@ -949,15 +1027,15 @@ void MsckfVio::PhotometricMeasurementJacobian(
     dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose();
 
     //dh / d X_{pl}
-    dh_dXplj.block<2, 3>(3, 0) = dh_dCpij * skewSymmetric(point);
+    dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point);
-    dh_dXplj.block<2, 3>(3, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose();
+    dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose();
 
     //d{}^Gp_P{ij} / \rho_i
     double rho = feature.anchor_rho;
     dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
 
-    dGpi_XpAj.block<3, 3>(3, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho)));
+    dGpi_XpAj.block<3, 3>(0, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho)));
-    dGpi_XpAj.block<3, 3>(3, 3) = Matrix<double, 3, 3>::Identity();
+    dGpi_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
     // Intermediate Jakobians
     H_rhoj = dI_dhj * dh_dGpij * dGpi_drhoj; // 1 x 3
@@ -973,9 +1051,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   // calculate residual
 
-  // visu -residual
-  //printf("-----\n");
-
   //observation
   const Vector4d& z = feature.observations.find(cam_state_id)->second;
 
@@ -995,16 +1070,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   for(int i = 0; i < photo_z.size(); i++)
     photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
 
-  // visu- residual
-  //for(int i = 0; i < photo_z.size(); i++)
-  //  printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
-
-  //Final Jakobians
-  // cout << "------------------------" << endl;
-  // cout << "rho" << H_rho.rows() << "x" << H_rho.cols() << "\n" << H_rho << endl;
-  // cout << "l" << H_pl.rows() << "x" << H_pl.cols()  << "\n" << H_pl << endl;
-  // cout << "A" << H_pA.rows() << "x" << H_pA.cols()  << "\n" << H_pA << endl;
-
   MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
   MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
 
@@ -1033,46 +1098,14 @@ void MsckfVio::PhotometricMeasurementJacobian(
     H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
   H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho;
 
-  // Original calculation
+  H_x = H_xl;
+  H_y = H_yl;
 
-  // Convert the feature position from the world frame to
+  //TODO make this more fluent as well
-  // the cam0 and cam1 frame.
+  count = 0; 
-  Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
+  for(auto data : photo_r)
-  Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w);
+  r[count++] = data;
 
-  // Compute the residual.
-  r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
-      p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
-
-
-// Compute the Jacobians.
-  Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
-  dz_dpc0(0, 0) = 1 / p_c0(2);
-  dz_dpc0(1, 1) = 1 / p_c0(2);
-  dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
-  dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2));
-
-  Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
-  dz_dpc1(2, 0) = 1 / p_c1(2);
-  dz_dpc1(3, 1) = 1 / p_c1(2);
-  dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2));
-  dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));
-
-  Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
-  dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
-  dpc0_dxc.rightCols(3) = -R_w_c0;
-
-  Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
-  dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0);
-  dpc1_dxc.rightCols(3) = -R_w_c1;
-
-  Matrix3d dpc0_dpg = R_w_c0;
-  Matrix3d dpc1_dpg = R_w_c1;
-
-  H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
-  H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;
-
-  photo_z.clear();
   return;
 }
 
@@ -1083,6 +1116,7 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   const auto& feature = map_server[feature_id];
 
+  int N = 13;
   // Check how many camera states in the provided camera
   // id camera has actually seen this feature.
   vector<StateIDType> valid_cam_state_ids(0);
@@ -1094,36 +1128,38 @@ void MsckfVio::PhotometricFeatureJacobian(
   }
 
   int jacobian_row_size = 0;
-  jacobian_row_size = 4 * valid_cam_state_ids.size();
+  jacobian_row_size = N * N * valid_cam_state_ids.size();
 
-  MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size,
+  MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
-      21+state_server.cam_states.size()*6);
+      21+state_server.cam_states.size()*7);
-  MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3);
+  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1);
-  VectorXd r_j = VectorXd::Zero(jacobian_row_size);
+  VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
-
   // visu - residual
-  printf("_____FEATURE:_____\n");
+  //printf("_____FEATURE:_____\n");
   // visu - feature
   //cam0.featureVisu.release();
 
   for (const auto& cam_id : valid_cam_state_ids) {
 
-    Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
+    //Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
-    Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
+    //Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
-    Vector4d r_i = Vector4d::Zero();
+    MatrixXd H_xl;
-    PhotometricMeasurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);
+    MatrixXd H_yl;
+    Eigen::VectorXd r_l = VectorXd::Zero(N*N);
+
+    PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
 
     auto cam_state_iter = state_server.cam_states.find(cam_id);
     int cam_state_cntr = std::distance(
         state_server.cam_states.begin(), cam_state_iter);
 
     // Stack the Jacobians.
-    H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi;
+    H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
-    H_fj.block<4, 3>(stack_cntr, 0) = H_fi;
+    H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
-    r_j.segment<4>(stack_cntr) = r_i;
+    r_i.segment(stack_cntr, N*N) = r_l;
-    stack_cntr += 4;
+    stack_cntr += N*N;
   }
   // visu - feature
   /*
@@ -1141,13 +1177,14 @@ void MsckfVio::PhotometricFeatureJacobian(
   */
 
   // Project the residual and Jacobians onto the nullspace
-  // of H_fj.
+  // of H_yj.
-  JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
+
+  JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
   MatrixXd A = svd_helper.matrixU().rightCols(
       jacobian_row_size - 3);
 
-  H_x = A.transpose() * H_xj;
+  H_x = A.transpose() * H_xi;
-  r = A.transpose() * r_j;
+  r = A.transpose() * r_i;
 
   return;
 }
@@ -1290,6 +1327,7 @@ void MsckfVio::measurementUpdate(
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
   VectorXd r_thin;
+  cout << " measurement update ..."  << endl;
 
   if (H.rows() > H.cols()) {
     // Convert H to a sparse matrix.
@@ -1480,12 +1518,18 @@ void MsckfVio::removeLostFeatures() {
     MatrixXd H_xj;
     VectorXd r_j;
     PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j);
-
     if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
+      cout << "made gating test" << endl;
     }
+    else
+    {
+      cout << "failed gating test" << endl;
+    }
+
+    cout << " stacked features up" << endl;
 
     // Put an upper bound on the row size of measurement Jacobian,
     // which helps guarantee the executation time.
@@ -1629,17 +1673,26 @@ void MsckfVio::pruneCamStateBuffer() {
     MatrixXd H_xj;
     VectorXd r_j;
 
+    cout << "getting featureJacobian...";
     PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
+    cout << "done" << endl;
+
     if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
+      cout << "made gating test" << endl;
+    }
+    else
+    {
+      cout << "failed gating test" << endl;
     }
-
     for (const auto& cam_id : involved_cam_state_ids)
       feature.observations.erase(cam_id);
   }
 
+  cout << " stacked features up" << endl;
+
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
 
@@ -1728,6 +1781,10 @@ void MsckfVio::onlineReset() {
   nh.param<double>("initial_covariance/extrinsic_translation_cov",
       extrinsic_translation_cov, 1e-4);
 
+  // Reset the irradiance covariance
+  nh.param<double>("initial_covariance/irradiance_frame_bias",
+      irradiance_frame_bias, 0.1);
+
   state_server.state_cov = MatrixXd::Zero(21, 21);
   for (int i = 3; i < 6; ++i)
     state_server.state_cov(i, i) = gyro_bias_cov;