CKF简单测试

CKF流程

对于k时刻的状态${\hat{\mathbf{X}}}_k$以及协方差${\mathbf{P}}_k$，令$L=dim({\hat{\mathbf{X}}}_k)$，CKF执行以下两步：

预测

定义矩阵$\xi$为：
$$\xi =\sqrt{L}\left[\begin{array}{cc}{\mathbf{I}}_L& -{\mathbf{I}}_L\end{array}\right]$$
计算容积点：${\mathbf{S}}_k=\sqrt{{\mathbf{P}}_k}$，对于第i个采样点，有：
$${\chi }_k^i={\hat{\mathbf{X}}}_k+{\mathbf{S}}_k{\xi }_i$$
对每个容积点进行状态传播得到k+1时刻的预测状态以及协方差：
$$\begin{array}{rl}{\chi }_{k+1|k}^i& =f({\chi }_k^i),i=1,2,...,2L\\ {\hat{\mathbf{X}}}_{k+1|k}& =\frac{1}{2L}\sum _{i=1}^{i=2L}{\chi }_{k+1|k}^i\\ {\mathbf{P}}_{k+1|k}& =\frac{1}{2L}\sum _{i=1}^{i=2L}({\chi }_{k+1|k}^i-{\hat{\mathbf{X}}}_{k+1|k})({\chi }_{k+1|k}^i-{\hat{\mathbf{X}}}_{k+1|k}{)}^T+{\mathbf{Q}}_{k+1}\end{array}$$

更新

同样对协方差进行分解得到${\mathbf{S}}_{k+1}$，进而得到每个容积点${\chi }_{k+1}^i$，并对量测进行预测：
$$\begin{array}{rl}{\mathbf{Z}}_{k+1}^i& =h({\chi }_{k+1}^i)\\ {\hat{\mathbf{z}}}_{k+1}& =\frac{1}{2L}\sum _{i=1}^{i=2L}{\mathbf{Z}}_{k+1}^i\end{array}$$
之后对量测的协方差以及状态-量测互协方差进行计算：
$$\begin{array}{rl}{\mathbf{P}}_{k+1}^{zz}& =\frac{1}{2L}\sum _{i=1}^{i=2L}({\mathbf{Z}}_{k+1}^i-{\hat{\mathbf{z}}}_{k+1})({\mathbf{Z}}_{k+1}^i-{\hat{\mathbf{z}}}_{k+1}{)}^T+{\mathbf{R}}_{k+1}\\ {\mathbf{P}}_{k+1}^{xz}& =\frac{1}{2L}\sum _{i=1}^{i=2L}({\chi }_{k+1}^i-{\hat{\mathbf{X}}}_{k+1|k})({\mathbf{Z}}_{k+1}^i-{\hat{\mathbf{z}}}_{k+1}{)}^T\end{array}$$
由此得到卡尔曼增益${\mathbf{K}}_{k+1}={\mathbf{P}}_{k+1}^{xz}({\mathbf{P}}_{k+1}^{zz}{)}^{-1}$，执行正常的滤波更新。

IMU方向估计

ImuFusion提供的orien.m程序中，利用陀螺仪数据做预测，加速度数据做方向更新。将其修改为CKF的滤波形式：

% ImuFusion based on CKF
for k = 2 : N
    
    %--- 1. Propagation --------------------------
    % Gyroscope measurements
    w = (imu_data(k-1, 2:4) + imu_data(k, 2:4))/2;
    w = w - bias_w;
    
    % Compute the F matrix
    Omega =[0     -w(1)   -w(2)   -w(3);...
            w(1)  0       w(3)    -w(2); ...
            w(2)  -w(3)   0        w(1); ...
            w(3)  w(2)    -w(1)    0  ];
    F = eye(4) + deltat * Omega / 2;
    
    % Compute the process noise Q
    G = [-x(2)  -x(3)  -x(4); ...
          x(1)  -x(4)   x(3); ...
          x(4)   x(1)  -x(2); ...
         -x(3)   x(2)   x(1)] / 2;
    
    % Propagate the state and covariance
    S1 = chol(P)';
    x_axis = (quat2rod(x'))';
    xsigmak = S1*kesai + repmat(x_axis,1,2*L);
    for i = 1 : 2*L
        xsigmak(:,i) = (quat2rod((F*rod2quat(xsigmak(:,i)')')'))';
    end
    xpre = sum(xsigmak, 2)/(2*L);
    P = 1/(2*L) * (xsigmak*xsigmak') - xpre * xpre' + Q;
    
    %--- 2. Update----------------------------------
    % Accelerometer measurements
    a = imu_data(k, 5:7);
    a = a - bias_a;
        
    % We use the unit vector of acceleration as observation
    ea = a' / norm(a);
    % Estimate the square-root factor of the predicted error covariance matrix
    S2 = chol(P)';
    xpre_sigmak = S2*kesai + repmat(xpre, 1, 2*L);
    zsigmak = zeros(3, 2*L);
    for i = 1 : 2*L
        q_pre = rod2quat(xpre_sigmak(:,i)');
        zsigmak(:,i) = [2*(q_pre(2)*q_pre(4) - q_pre(1)*q_pre(3)); ...
                        2*(q_pre(3)*q_pre(4) + q_pre(1)*q_pre(2)); ...
                        q_pre(1)^2-q_pre(2)^2 - q_pre(3)^2+q_pre(4)^2];
    end
    zpre = sum(zsigmak, 2)/(2*L);
        
    % Measurement noise R
    R_internal = (noise_accel / norm(a))^2 * eye(3);
    R_external = (1-gravity/norm(a))^2 * eye(3);
    R = R_internal + R_external;
    
    % residual, measurement covariance
    Pzz = 1/(2*L)*(zsigmak*zsigmak') - zpre*zpre' + R;
    Pxz = 1/(2*L)*(xpre_sigmak*zsigmak') - xpre*zpre';
    y = ea - zpre;
    
    % Update
    K = Pxz/Pzz;
    x = rod2quat((xpre + K*y)')';
    P = P - K*Pzz*K';
    
    % 3. Ending
    x = x / norm(x);    % Normalize the quaternion
    P = (P + P') / 2;   % Make sure that covariance matrix is symmetric
    allX(k,:) = x';     % Save the result
    
end

测试结果：
估计方向曲线：

RMSE (deg)	EKF	CKF
yaw	0.3953	0.4011
pitch	0.3782	0.2349
roll	0.4412	0.3438

每帧耗时：EKF: 0.024 ms， CKF: 0.15ms。
CKF的精度虽高，但是耗时较之EKF远甚。

参考资料

1. ImuFusion参考代码 https://github.com/meyiao/ImuFusion
2. IMU方向估计 https://blog.csdn.net/waihekor/article/details/103748333
3. CKF原理 https://zhuanlan.zhihu.com/p/554143609