MATLAB的卡尔曼滤波函数与实例

卡尔曼滤波是很很常用的预测和估计方法，自己学习了也挺久，这里将自己写的卡尔曼滤波函数分享一下。卡尔曼滤波的讲解网上有很多，这里不对此进行叙述，只是对MATLAB中的函数做讲解。函数主体来自https://blog.csdn.net/u012936940/article/details/76805526，我在此基础上进行了补充和注释，并且写成了函数。链接里面也有关于卡尔曼滤波原理的解释，说明简单清晰，我看了以后也有了更深的理解。

代码部分

function  Xkf= KaermanFilter(InitX,Z,F,Q,H,R,P0)
%KAERMANFILTER 此处显示有关此函数的摘要
%参数说明： InitX:X的初始值,不知道初始值就输入零向量
%           Z:输入测量得到的数据
%           F：状态转移矩阵
%           H：观测矩阵
%           Q：协方差矩阵，反映状态受外界影响的大小
%           R：测量协方差矩阵，反映测量的精度，取决于传感器
%读取数据
X=InitX;
%数据点数目
[l,s]=size(X);%获取数据维数
L=length(Z);
%初始化观测值矩阵 
Xkf=zeros(L,s);
Xkf(:,1)=X(:,1);
P=P0;
%滤波 
for i=2:L
    Xn=F*(Xkf(i-1,:)');%一步预测
   P=F*P*F'+Q;%一步预测误差方差阵
    K=P*H'*inv(H*P*H'+R);;%滤波增益矩阵（权重）
    Xkf(i,:)=Xn+K*(Z(i,:)-H*Xn);%状态误差方差阵估计
    P=(eye(s)-K*H)*P;
end 
fig=figure(1);
set(fig,'position',[200 200 1200 500]);
hold on;
plot(Z(:,1),'-b.','MarkerSize',1);
hold on;
plot(Xkf(:,1),'-r','MarkerSize',1);
legend('观测轨迹','滤波轨迹');
end

结合例题讲解

其实理解卡尔曼滤波函数，主要就是理解中间的递推过程，而关键的就是理解各个参数是什么意思，或者记住怎么计算就可以了。

例题一

例题一比较简单，是我看的书上的一个例子，可以自己手算。
先给出题目和题目的解答：
（中间的delta表示克罗尼克函数，就是i和j相等的时候为1，不等的时候为0）



然后将相应条件转换为MATLAB中的矩阵

X=1;
Z=[0 2 4 3 2]';%题中X是从零时刻开始，但是Z零时刻的值不影响计算，因为X零时刻X的值是准确值，Z(0)不需要参与计算
Q=2;
R=1;
P=4;
A=2;
H=1;
KaermanFilter(X,Z, A, Q, H, R,P)

结果：
Z(0)为0时候的结果：

Z(0)为1时候的结果：

可以看到，计算的结果和手算的是一样的，并且Z(0)的取值不影响最终的结果。

例题二

例题二的数据来源于真实测量的数据，是人体运动时候盆骨的坐标，这个例子可能更接近实际应用。
ps：这里数据是师兄采集的，我只是借用一下其中一组。
链接: https://pan.baidu.com/s/1EpoCpGluIeNkeruDpZ8XXA 提取码: ccgq

clf
Z=data;
r=0.0005;
for i=1:length(Z)
	Z(i)=data(i)+normrnd(0,1)*r;
end
D=Z-data;
X=Z(1);
T=0.001;
V=0.1;
Q=T^2*V^2 ;
R=r^2;
P=X^2;
A=1;
H=1;
Xn=KaermanFilter(X,Z, A, Q, H, R,P);
plot(data,'g');
RMS(Z,data);
RMS(Xn,data);

对参数的选择做简单的说明，这里的单位是米。
由于数据已经经过滤波，所以将数据作为理想的轨迹，在理想轨迹上加偏差得到采样值。
由于人运动的状态方程是非线性的，且是未知的，把人的速度当成是一个随机的变量处理
因此得到递推方程
$$X(k+1)=AX(k)+V\times v(k)\times \Delta T$$
将速度视为随机变量，服从正态分布，方差为1，采样的时间间隔为0.001秒，考虑人体运动的速度，取V为适当的速度幅值，这里为0.1。于是得到方程
$$X(k+1)=X(k)+0.0001\times w(k)$$

$$Z(k)=X(k)+u(k)\ast r$$
其中$$w(k)和v(k)$$都是标准正态分布，r是采集的精度，取0.001。

结果

其实结果表现得不是十分好，因为本身这个数据用卡尔曼滤波来做不是十分合适，只是为了使用卡尔曼滤波而是用卡尔曼滤波。一般的卡尔曼滤波用在已知的线性系统里还是比较好用的。

最后

这次主要是对卡尔曼滤波进行一下个人总结，在写博客的过程中，对此理解又加深了不少， 也希望可以帮助到其他人。