MSCKF

主要数据结构：StateServer当前里程计状态

// imu状态、若干相机位姿、状态协方差矩阵、噪声协方差
struct StateServer {
  IMUState imu_state;
  CamStateServer cam_states;

  // State covariance matrix
  Eigen::MatrixXd state_cov;
  Eigen::Matrix<double, 12, 12> continuous_noise_cov;
};

主要函数：

initialize() {
	loadParameters() //读取参数，初始化噪声和协方差
	createRosIO()->{	 //发布和订阅topic
		//保存IMU数据，初始化重力和IMU初始方向
		imu_sub = nh.subscribe("imu", 100,
      		&MsckfVio::imuCallback, this);
      	//MSCKF里程计
  		feature_sub = nh.subscribe("features", 40,
      		&MsckfVio::featureCallback, this);
	}
}

void MsckfVio::featureCallback(const CameraMeasurementConstPtr& msg) {
   	@第一帧图像帧为初始帧
   	
   	//对当前帧之前的imu_msg_buffer里所有IMU数据进行处理，得到当前state
   	batchIMUProcessing()->{	
   		for (const auto& imu_msg : imu_msg_buffer) 
   			processModel(imu_time, imu_msg.gyro, imu_msg.acc)->{
   				@IMU量减去bias
   				
   				//误差状态传递方程的两个矩阵 x' = F * x + G * n
   				@根据附录A计算F
   				@根据附录A计算G
   				
   				//F和G是连续时间的误差方程，需要离散化
   				//x(k+1) = Phi * x(k) + W(k)
   				@其中Phi = e^(F*dt) = I + F * dt + 0.5 * F^2 * dt^2 + ...
   				
   				// RK4阶积分传递imu误差状态
   				predictNewState(dtime, gyro, acc)->{
   					@计算Omega矩阵，四元数求导公式[见注1]：  q' = 0.5 * Omega * q
   					
   					@imu当前状态p,v,q
   					
   					@RK4积分pvq，yn+1 = yn + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
   				}
   				
   				@修正状态转移矩阵Phi
   				
   				// 噪声协方差传递
   				Q = Phi * G * state_server.continuous_noise_cov * G.T * Phi.T * dtime;
   				// 状态协方差传递
   				state_server.state_cov = Phi * state_server.state_cov * Phi.T + Q;
   				
   				@若state中存在cam状态，则更改state_cov的相应块
   				@更新state时间戳
   			}
   		end for
        @更新state id
   		@清空用过的imu数据
   	}
   	
   	//预测当前cam位姿并加到msckf的state中，对系统的协方差矩阵进行增广
   	stateAugmentation(msg->header.stamp.toSec())->{
   		//根据imu-cam外参和imu状态估计cam位姿
   		R_w_c = R_i_c * R_w_i;
   		t_c_w = state_server.imu_state.position + R_w_i.T * t_c_i;
   		@将cam位姿加到state_server中
   		
   		@计算增广的cam位姿对已有状态的雅可比
   		@协方差矩阵增广
   		//      [ I(21+6N) ]               [ I(21+6N) ]^T
  		//  P = [          ] * [P11 P12] * [          ]
  		//      [    J J0  ]   [P21 P22]   [    J J0  ]
   	}
   	
   	//更新特征观测：新特征加入到map中，老特征跟踪次数加1
   	addFeatureObservations(msg);
   	
   	//update的两种情况之一：观测大于3的特征跟踪丢失
   	// 剔除不能三角化且观测次数过少的特征点，同时计算残差和雅可比
   	removeLostFeatures()->{
   		//剔除观测次数小于3和不能三角化的特征点
   		@将满足上述条件的特征点加入到invalid_feature_ids，并剔除
   		@否则加入到processed_feature_ids
   		
   		//雅可比维度H_x：(4M-3)*(21+6*N), 残差维度r：(4M-3) 
   		// r = z - H * x
   		@计算processed_feature_ids中特征点的雅可比和残差
   		for (const auto& feature_id : processed_feature_ids)
   			featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
   		
   		//测量更新 
   		// S = H * P * H^T + R
   		// K = P * H^T * S^(-1)
   		// x_new = x + K * r
   		// P_new = (I - K * H) * P
   		measurementUpdate(H_x, r);
   		// 移除processed_feature_ids特征点
   		for (const auto& feature_id : processed_feature_ids)
    		map_server.erase(feature_id);
   	}
   	
   	//update的两种情况之二：slideWindow满了
   	pruneCamStateBuffer()->{
   		//移除最老帧或次新帧
   		findRedundantCamStatus(rm_cam_state_ids);
   		
   		//雅可比维度H_x：(4M-3)*(21+6*N), 残差维度r：(4M-3) 
   		// r = z - H * x
   		@计算processed_feature_ids中特征点的雅可比和残差
   		for (const auto& feature_id : processed_feature_ids)
   			featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
   			
   		//测量更新
   		measurementUpdate(H_x, r);
   		
   		@将cam的对应块从state_cov中剔除
   		@将cam状态从state_server中剔除
   	}
   	
   	//发布里程计到rviz
   	publish(msg->header.stamp);
}

注1：四元数求导
$$\begin{gathered} 令q_a=[s_a,w_a],q_b[s_b,w_b],s为虚部，w为实部 \\ 四元数乘法：q_a\otimes q_b=R(q_b)q_a, \\ 其中，R(q_b)=\left[\begin{array}{cc}-[w_b{]}_{\times }& w_b\\ -w_b^T& 0\end{array}\right]+s_{b}I={\Omega }_b+s_{b}I \\ \begin{array}{rl}则四元数导数为：{\dot{q}}_t& =\underset{dt\to 0}{\lim }\frac{1}{dt}(q_{t+dt}-q_t)\\ & =\underset{dt\to 0}{\lim }\frac{1}{dt}(q_t\otimes q_{t+dt}^t-q_t\otimes \left[\begin{array}{c}0\\ 1\end{array}\right])\\ & =\underset{dt\to 0}{\lim }\frac{1}{dt}(q_t\otimes \left[\begin{array}{c}k\sin \theta /2\\ \cos \theta /2\end{array}\right]-q_t\otimes \left[\begin{array}{c}0\\ 1\end{array}\right])\\ & =\underset{dt\to 0}{\lim }\frac{1}{dt}(q_t\otimes \left[\begin{array}{c}k\theta /2\\ 1\end{array}\right]-q_t\otimes \left[\begin{array}{c}0\\ 1\end{array}\right])\\ & =\underset{dt\to 0}{\lim }\frac{1}{dt}(R(\left[\begin{array}{c}k\theta /2\\ 1\end{array}\right])-R(\left[\begin{array}{c}0\\ 1\end{array}\right]))q_t\\ & =\underset{dt\to 0}{\lim }\frac{1}{dt}\left[\begin{array}{cc}[-\frac{\theta }{2}{]}_{\times }& \frac{\theta }{2}\\ -\frac{{\theta }^T}{2}& 0\end{array}\right]q_t\end{array} \\ 又有角速度w=\underset{dt\to 0}{\lim }\frac{\theta }{dt}，因此上式最终化简为： \\ {\dot{q}}_t=\frac{1}{2}\Omega (w)q_t=\frac{1}{2}{q}_t\otimes \left[\begin{array}{c}w\\ 0\end{array}\right] \end{gathered}$$