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：四元数求导
令 q a = [ s a , w a ] , q b [ s b , w b ] , s 为 虚 部 ， w 为 实 部 四 元 数 乘 法 ： q a ⊗ q b = R ( q b ) q a , 其 中 ， R ( q b ) = [ − [ w b ] × w b − w b T 0 ] + s b I = Ω b + s b I 则 四 元 数 导 数 为 ： q ˙ t = lim ⁡ d t → 0 1 d t ( q t + d t − q t ) = lim ⁡ d t → 0 1 d t ( q t ⊗ q t + d t t − q t ⊗ [ 0 1 ] ) = lim ⁡ d t → 0 1 d t ( q t ⊗ [ k sin ⁡ θ / 2 cos ⁡ θ / 2 ] − q t ⊗ [ 0 1 ] ) = lim ⁡ d t → 0 1 d t ( q t ⊗ [ k θ / 2 1 ] − q t ⊗ [ 0 1 ] ) = lim ⁡ d t → 0 1 d t ( R ( [ k θ / 2 1 ] ) − R ( [ 0 1 ] ) ) q t = lim ⁡ d t → 0 1 d t [ [ − θ 2 ] × θ 2 − θ T 2 0 ] q t 又 有 角 速 度 w = lim ⁡ d t → 0 θ d t ， 因 此 上 式 最 终 化 简 为 ： q ˙ t = 1 2 Ω ( w ) q t = 1 2 q t ⊗ [ w 0 ] 令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)=\begin{bmatrix} -[w_b]_{\times} & w_b\\-w_b^T &0 \end{bmatrix}+s_bI=\Omega_b+s_bI \\ \begin{aligned} 则四元数导数为：\dot q_t&=\lim_{dt\rightarrow0}\frac{1}{dt}(q_{t+dt}-q_t)\\ &=\lim_{dt\rightarrow0}\frac{1}{dt}(q_{t}\otimes q^t_{t+dt}-q_t\otimes\begin{bmatrix}0\\1\end{bmatrix})\\ &=\lim_{dt\rightarrow0}\frac{1}{dt}(q_{t}\otimes \begin{bmatrix}k\sin\theta/2\\\cos\theta/2\end{bmatrix}-q_t\otimes\begin{bmatrix}0\\1\end{bmatrix}) \\ &=\lim_{dt\rightarrow0}\frac{1}{dt}(q_{t}\otimes \begin{bmatrix}k\theta/2\\1\end{bmatrix}-q_t\otimes\begin{bmatrix}0\\1\end{bmatrix}) \\ &=\lim_{dt\rightarrow0}\frac{1}{dt}(R(\begin{bmatrix}k\theta/2\\1\end{bmatrix})-R(\begin{bmatrix}0\\1\end{bmatrix}))q_t \\ &=\lim_{dt\rightarrow0}\frac{1}{dt}\begin{bmatrix}[-\frac{\theta}{2}]_{\times} & \frac{\theta}{2}\\-\frac{\theta^T}{2} & 0\end{bmatrix}q_t \end{aligned} \\ 又有角速度w=\lim_{dt\rightarrow0} \frac{\theta}{dt}，因此上式最终化简为：\\ \dot q_t=\frac{1}{2}\Omega(w)q_t=\frac{1}{2}q_t\otimes \begin{bmatrix} w \\ 0\end{bmatrix} 令qa​=[sa​,wa​],qb​[sb​,wb​],s为虚部，w为实部四元数乘法：qa​⊗qb​=R(qb​)qa​,其中，R(qb​)=[−[wb​]×​−wbT​​wb​0​]+sb​I=Ωb​+sb​I则四元数导数为：q˙​t​​=dt→0lim​dt1​(qt+dt​−qt​)=dt→0lim​dt1​(qt​⊗qt+dtt​−qt​⊗[01​])=dt→0lim​dt1​(qt​⊗[ksinθ/2cosθ/2​]−qt​⊗[01​])=dt→0lim​dt1​(qt​⊗[kθ/21​]−qt​⊗[01​])=dt→0lim​dt1​(R([kθ/21​])−R([01​]))qt​=dt→0lim​dt1​[[−2θ​]×​−2θT​​2θ​0​]qt​​又有角速度w=dt→0lim​dtθ​，因此上式最终化简为：q˙​t​=21​Ω(w)qt​=21​qt​⊗[w0​]