低成本MEMS INS系统 + GNSS组合导航序贯滤波MATLAB仿真

对于标准Kalman滤波，其中增益计算式涉及矩阵的求逆运算，当量测维数较高时，计算量很大。序贯滤波（sequential Kalman filter）是一种将高维数量测更新降低为多个低维数量测更新的方法，能有效地降低矩阵的求逆计算量。

序贯滤波与kalman滤波最大的区别在于，量测更新的过程：
滤波系统的随机状态空间模型为：

但是，这里假设在K时刻量测方程可以分解成如下N组：

且量测噪声之间互不相关，这时量测噪声方差阵可写成分块对角阵形式，即：

序贯滤波的流程框图如下图所示：

其中，量测更新过程分为N次的迭代更新，最终使得$X_k^{(N)}=X_k$，$P_k^{(N)}=P_k$：


编写量测更新程序具体如下：

        n=length(Zk);
        for k=1:n
            Rk0=kf.Rk(k,k);Zk0=Zk(k);Hk0=kf.Hk(k,:);
            kf.PXZkk_1 = kf.Pkk_1*Hk0';
            kf.PZkk_1 = Hk0*kf.PXZkk_1 + Rk0;
            kf.Kk = kf.PXZkk_1/kf.PZkk_1;
            kf.Xk = kf.Xkk_1 + kf.Kk*(Zk0-Hk0*kf.Xkk_1);
%             kf.Pk = kf.Pkk_1 - kf.Kk*kf.PZkk_1*kf.Kk';
            kf.Pk = kf.Pkk_1 - kf.Kk*Hk0*kf.Pkk_1;
            kf.Xkk_1=kf.Xk;
            kf.Pkk_1=kf.Pk;
        end

仿真结果：

完整的代码见：序贯滤波（sequential Kalman filter）+ MATLAB程序（简化版sins与gnss融合）