【卡尔曼滤波器】递归算法

  大家好，我是小政。最近在学习卡尔曼滤波，本篇文章记录一下我学习的卡尔曼滤波器中的递归算法，通过举例子让大家更加清晰理解递归到底是什么？希望与同是卡尔曼滤波研究方向的同学进行一些交流。

1.为什么要用卡尔曼滤波器？

  
现实社会中存在诸多的不确定性：

1. 不存在完美的数学模型
2. 系统的扰动不可控，也很难建模
3. 测量传感器存在误差

2.公式推导

  设测量结果为Zk，k表示第k次假设硬币真实直径50mm，我们测得的数据可能是Z1=50.1mm，Z2=50.3mm，Z3=49.8mm
  通常情况下，我们会把估计的真实数据进行一个取平均值(平均值法又叫均值滤波)。


  经过一系列推导我们可以得到：

  即：当前估计值=上一次估计值+卡尔曼增益×(当前测量值-上一次估计值)

  ★ 递归思想：新的估计值与上一次的估计值有关

  个人理解：测量误差是传感器的示值与传感器真正接受到的物理量之间的差值。

  进而得到，卡尔曼增益：

  讨论：在k时刻

  计算递归问题步骤如下：

3.举例说明(基于Matlab/Excel平台)

（1）Excel实验

  假设有20组数据，我们在Excel上进行实现得到：

（2）Matlab实验

%rand函数: 随机产生0到1内的一个数，rand(50,1)产生50个0到1内的数字
Z = 3*(rand(50,1)*2-1)+50;%产生47到53内的随机数字
x_hat= zeros(50,1);
K    = zeros(50,1);
e    = zeros(50,1);
x_hat(1) = 40;
e(1) = 5;
K(1) = 0;
%%
for k = 2:50
    K(k) = e(k-1)/(e(k-1)+3);
    x_hat(k) = x_hat(k-1)+K(k)*(Z(k)-x_hat(k-1));
    e(k) = (1-K(k))*e(k-1);
end
figure('NumberTitle', 'off', 'Name', 'Recursive Algorithm');
plot(Z);
hold on
plot(x_hat);
legend('测量值','估计值');

 

  
 

  运行结果如下图：