卡尔曼滤波的一阶、三阶应用实例，矩阵的初始化问题

五个基本公式

最近学习卡尔曼滤波，本想着从卡尔曼的那篇论文学起，但是奈何看了很长时间不知道到底在干什么（本人着实太菜了），后来无奈之下开始从国内的文献看起，结合各位大佬的博客，总算是有点理解了，所以打算记录下来，也与各位共享。
关于五个公式的作用网上已经有很多人讲的很清楚了，这里不再赘述，大家可以参考这篇中文论文，里边关于五个公式及一些参数的初始化问题讲的比较透彻。这里仅将公式贴出：

首先必须要指出公式（7）中黄色标注的部分是单位矩阵I，而不是数字1，我再进行高阶滤波时在这里卡了很久。

各个参数的初始化及由来

公式中需要重点讲解的参数一共有8个：A、B、Pk、Q、R、H、Kk、Zk.
A：状态空间方程中对应的系数矩阵
B：状态空间方程中的输入矩阵，一般在没有控制输入的情况下B=0
P_k：协方差矩阵，这个比较重要的是他的初始值，虽然最后都会收敛
Q：过程激励噪声协方差，是这里面难以确定的量，需要根据自己的实际过程进行优化
R：据说可以观测得到（我一般都是直接取0.05）
H:量测矩阵，和可观测变量有关，等下回进行具体的描述
K_k：卡尔曼增益，通过公式算出来的
Z_k：实际观测到的测量值

卡尔曼滤波实际应用的例子

一下分为一阶模型和三阶模型进行举例，由此可以向更高阶推广。

一阶过程的滤波实例

由于一阶滤波没有具体的物理模型，所以就令A=1，得到X_k_=X_k-1
B=0;
P_k=0.2;
X_k(1)=0;
Q=0.01;
R=0.05;
H=1; % 在一阶滤波下A和H都是取0的

具体程序如下：（MATLAB程序）

clf
clc
clear
% 对白噪声信号进行滤波
% 设置总时长为10s
% 采样频率为200hz
fs = 100;
T = 10;
n = T*fs+1;
t = linspace(0, 10, n);
yv = 10*sin(t)


% 指定要产生的信号的行数和列数，以及噪声信号的功率
% 产生白噪声
z = wgn(1, n, 0);


Pk_=0;
Pk=0.02;
Xk(1)=0;
Gk=0;
Q=0.01;
R=0.543;
% 在原来的完美值中加入噪声，模拟变成传感器测到的数据
real_input = yv + 2*z';
for i=2:n
    Pk_ = Pk + Q;
    Gk = Pk_/(Pk + R);
    Xk(i) = Xk(i-1)+Gk*(real_input(i)+ - Xk(i-1));
    Pk = (1-Gk)*Pk_;
end
% 将完美值、加入噪声的测量值、滤波值进行对比
plot(t,real_input,'r',t,Xk,'b',t,yv,'k')

三阶滤波的参数设置

这里以匀加速直线运动为例，其时间方程为
x=x₀+v₀t+0.5*a₀t²,转换到离散的状态空间方程可以得到
，x(k)=

这里着重需要将一下H的取值，在我的程序中，我假设只有运动物体的当前的位置是可以通过传感器测量的，所以H=[1 0 0],而如果，假设只有速度是可以测量的，那么H=[0 1 0],如果假设位置、速度都是可以测量的，那么就可以令H=[1 0 0;0 1 0],依次类推。
下面的程序利用卡尔曼滤波对传感器测量得到的位置信息进行滤波，并和一阶滤波的效果进行对比（MATLAB程序)

% 模拟一个10秒的匀加速直线运动
% 通过噪声来混乱运动，可观测量只有位置信息
clc
clf
clear
T=0.02;  % 时间间隔设为0.1
TT=10;  % 总时间为10s
n = TT/T+1;
t = linspace(0,TT,n);

s0=2.3;v0=0.5;a0=1;
s = s0+v0*t+(a0*t.^2)/2;
w = wgn(1, n,0);
s_dis = s+1*w;
v = v0+a0*t;
a = a0 + 0*t;
x = [s;v;a];

% 滤波器的一些参数
% 在已有模型的前提下对数据进行滤波
A=[1, T, (T^2)*0.5; 0, 1, T; 0, 0, 1];
Pk=[0.1, 0, 0; 0, 0.02, 0; 0, 0, 0.02];
R = 0.05;
Q = [0.0, 0, 0; 0, 0.1, 0; 0, 0, 0.01];
H = [1 0 0];
Xk(:,1)=x(:,1);
for i = 2:n
    Xk_ = A*Xk(:,i-1);
    Pk_=A*Pk*A' + Q;
    Kk = Pk_*H'/(H*Pk*H'+R);
    Xk(:,i) = Xk_ + Kk*(s_dis(i)-H*Xk_);
    Pk = (eye(3)-Kk*H)*Pk_;
end

% 在不考虑模型的前提下单独对被预测数据进行滤波
Pk_=0;
Pk=0.02;
Xkk(1)=0;
Gk=0;
Q=0.01;
R=0.543;
for i=2:n
    Pk_ = Pk + Q;
    Gk = Pk_/(Pk + R);
    Xkk(i) = Xkk(i-1)+Gk*(s_dis(i)+ - Xkk(i-1));
    Pk = (1-Gk)*Pk_;
end
plot(t,s_dis,'r',t,Xk,'b',t,s,'k')

plot(t,s,'r',t,Xk(1,:),'b',t,Xkk,'g',t,s_dis,'k')

目前看来一阶、三阶区别不大，可能使我一些参数的初始化不好，后续会继续改进，同时会继续学习扩展卡尔曼等变形算法，欢迎大家继续关注。