卡尔曼滤波算法，最小方差估计——前传

Angus · 发表于 2015-5-30 15:10:32

本帖最后由 Angus 于 2016-6-30 22:58 编辑

先说什么叫先验估计
现实生活中，某个物理量的变化，总会符合某个规律或公式，如电机转速与所加电压有关, 一天的气温与早上，中午，晚上等一天中的时间有关。单片机定时测量某物理量——转速或温度时，会得到一个数据序列：x(n), x(n-1), x(n-2)....，
   举个例子，比如我们每秒测量一次电机转速，得到的数据是：1000转，1000转，1000转，那估计下次测量值也是 1000转。若得到的数据是：999转，1000转，1001转，那估计下次是 1002转。
   根据以往的数据，用变化规率或公式，估计的下一个值，叫先验估计。

一般情况下，在 e(t) 的驱动之下，随时间变化的物理量 x(t) ，其变化规律可用微分方程表式为  dx(t)/dt +a*x(t) = b*e(t)。单片机测量值是一个离散序列，前后值相减，就是求微分，公式变成：【x(n+1) - x(n)】 + a*x(n) = b*e(n)。一般简化成这样的通式: x(n+1) = A*x(n) + B*e(n)。大多数物理量，都可以根据当前值 x(n) 和当前的输入激励，估算出下一时刻的值 x(n+1)，若x(n)是一个包含随机量（干扰）的估计值，算出的 x(n+1)的值，就是＂先验估计值＂。

测量值总会因干扰有波动，比如一杯水的温度，测量值是 50.5度，50.8度，51.6度，50.7度.....
按数据规律，估计下一时刻的温度是51.0度——先验估计A，到了下一时刻点，测出来的值却是52.0度——测量值B，那么哪一个值更准确呢？ A 还是 B ？

我们可以认为二者的平均值更接近真实值，或者三七开，二八开，写成公式： Y = (1-K)*A + K*B ， K 取值 0~1，Y 就在 A ~ B 之间。K=0时，Y=A。K=1时，Y=B。这里 Y 也是估计值，由测量值校正过的估计值，所以叫后验估计。每次得到测量值后，算出一个 Y 值，  Y 值就组成一个序列： Y(n), Y(n-1), Y(n-2), .......公式写成 Y = A + K*(B-A) 少做一次乘法。

干扰是个随机量，Y 序列方差最小的时候，应是 Y 序列的最优估计。1960年，卡尔曼发表论文，数学公式推导了一个可以让 Y 值序列方差最小的 K 值递推方法，人们称之为：卡尔曼滤波算法，这个算法的好处是，每次测量后递推，运算量很小，能有效滤除随机量。现在，卡尔曼滤波算法，广泛用于运动物体的轨迹估计：测量宇宙飞船的飞行轨迹，测量敌方飞机或导弹运动轨迹，电机 FOC 控制磁钢位置的测量估算，陀螺仪转角估算，等等。