自动驾驶-序惯卡尔曼滤波【附Github源代码】

经验表明,估计和控制应用领域的工程师更重视在计算机上简单地实现卡尔曼滤波算法,而没有考虑过卡尔曼滤波算法的计算误差案例。
——Gerald Bierman和 Catherine Thornton[ Bie77a]

1引言

本文着眼于卡尔曼滤波方程的一些其他形式,有一些卡尔曼滤波方程在数学上是等价的。在查阅不同的文献或书籍时,人们可能会发现完全不同的卡尔曼滤波方程的表示形式,这些差异可能是印刷错误,也可能它们在数学上是等价的
难证明,所以不同等式等价性的证明结果很难得到。
讨论的第一种方法称为序贯卡尔曼滤波。序贯卡尔曼滤波器的优点是不必求解矩阵的逆,这在不具备矩阵函数库的嵌入式系统中非常适用,当然它也需要满足某些条件才能使用。
序贯卡尔曼滤波器。这是一种不求矩阵逆的卡尔曼滤波方式。这在没有矩阵运算程序的嵌入式系统中优势显著。但必须注意的是,序贯卡尔曼滤波也需要满足一些限制条件。
总之,序贯卡尔曼滤波只有在满足下列两个条件之一时才有意义
1．量测噪声协方差R,是对角阵;
2．量测噪声协方差R是常数阵。
最后,请注意有时也称卡尔曼滤波为序贯滤波,序贯通常用来表征递归[ Buc68，第3章;Bro96]。这可能会导致一些术语的混淆。**然而,序贯滤波也经常在文献中使用,即贯滤波是一次只处理一个量测值的滤波方法(而不是将量测值作为一个整体向量来进处理)。**有时候,标准卡尔曼滤波被称为批量卡尔曼滤波以区分序贯卡尔曼滤波。
序惯卡尔曼滤波主要使用在计算量较小且没有相关求矩阵逆的MCU上，看到此算法不错，mark，后续如用到再回来看下。

2关于序惯卡尔曼滤波算法的推导

我个人的理解：其实这部分的公式推导均是基于基本的卡尔曼滤波算法，这个序惯卡尔曼滤波算法最鲜明的特征就是不用卡尔曼增益K的求解无需求矩阵逆了，他本质并没有改变卡尔曼算法的卡尔曼增益的求解方式，而是通过把向量矩阵进行了分解，这样矩阵求逆就是标量的除法，所以通过这样的手段将矩阵求逆运算变成了标量的除法。

3 书中所给的例子

4 笔者个人的理解使用MATLAB实现序列卡尔曼滤波算法

我使用MATLAB写了个序列卡尔曼滤波算法来求解上述题目：
序惯卡尔曼滤波的核心代码如下（SeqKalmanFilter.m）：

%@Function：本程序主要实现序惯卡尔曼滤波算法
%@Author：Juchunyu
%@Date：2022-2-4 09:18:00
function [pRes kRes x] =SeqKalmanFilter(F,H,Q,v,R,P0,x0,y,meassureNum,x)
    pRes = [];
    kRes = [];
    xRes=[];
    %predict model
    P = F*P0*F'+Q
    X = F*x0
    for i=1:meassureNum
        %参数初始化处理
        if i==1
            p=P(1,1);
        else
            p=P_;
        end  
        [mh,nh]=size(H);
        if(i>mh)
            h=0;
        else
            h=H(i,:) 
        end
        r= R(i,i)
        %计算测量更新
        k = p*h/(h*p*h'+r)%卡尔曼增益
        kRes = [kRes;k];%记录卡尔曼增益k的计算结果
        xPredict = x(i)+k*(y(i,:)-h*x(i,1))%卡尔曼预测出的状态
        x=[x;xPredict]
        [M,N] = size(k*h)
        P_ = (ones(M,N)-k*h)*p   %卡尔曼输出的协方差
        pRes=[pRes;P_];%记录卡尔曼协方差的计算结果
    end
end

如何运行？执行下述的代码即可(MainSeqKalmanFilter)：

clear
%%%%参数设置%%%%
F=[0.95];%%状态矩阵
H = [1 0.2 0.02]';%观测矩阵
Q = [2];   
v=[2 1 50]
R = diag(v); 
%初始值设置
P0 = 4;
x0 = 1;
x = [x0];
%测量值的数量
meassureNum = 3;
%%观测值
y = [6 3 -100]';
[pRes0 kRes xRes] =SeqKalmanFilter(F,H,Q,v,R,P0,x0,y,meassureNum,x);

MATLAB能实现，其他语言也可以实现。
另外，这个MATLAB的代码我是根据自己对算法的理解写的，如有错误，请指出。
github代码链接：https://github.com/JackJu-HIT/SeqKalman
如果觉得文章还不错，可以关注下我的微信公众号：

                                                                                      20220204
                                                                                                      Juchunyu