卡尔曼滤波改进算法详细推导（对卡尔曼滤波的理解和初步计算）

首先卡尔曼滤波是用来帮助测量的，是为了让测量结果尽可能的逼近真实值。

看到一个很好的例子来说明卡尔曼滤波是什么，在航海中，为了得到船的当前位置，航海长通常用前一时刻的船位置为基准，根据航向、航速和海流推算出下一个船位，我们称之为估计船位；但是他不能轻易认为这个位置就是正确的位置，他还要通过适当的仪器测量得到另一个船位，称之为测量船位（我们的测量仪器也不准）。这两个船位一般不重合，航海长需要通过分析和判断（或者说是用测量对估计船位进行修正）选择一个最优的估计船位作为船的当前位置。那么这个分析决策的过程就是卡尔曼滤波。

算法的核心思想是根据当前的仪器"测量值" 和上一刻的 "估计值" 和 "误差",计算得到当前的最优量。

更具体的计算以我们的机器人运动中对朝向的估计为例，通过K-1时刻的位置和里程计的变化，我们获得了K时刻的朝向A1=30（估计值） ,同时我们K时刻里程计的误差（噪声）是W1=5度（后面有具体计算的方法），同时我们通过惯性测量单元（IMU）可以获得K时刻的朝向值A2=40（测量值）以及他的误差（噪声）是W2=2度。那么我们现在有两个朝向值，我们信哪一个呢？我们引入卡尔曼增益这个变量G，计算公式：G*G = W1*W1 / (W1*W1 W2*W2) ，从而得到G = 0.9285。那么最终K时刻的朝向就是：A1 G*(A2-A1) =30 0.9285*(40-30) = 39.285。可以看出最终值是更接近误差较小的测量值。

同理K 1时刻的最终值可以根据K时刻的数据来递归推导。最后如何计算K 1时刻里程计误差W3呢（同样W1是怎么来的呢）？首先我们需要计算K时刻的最优估计误差，其计算公式是：Yk = (I-Gk*H)*Wk-1. 本例就是：Y=sqrt((1-G)*W1*W1) = 1.337, 另外还有一个是你对K 1时刻估计的不确定度V=4，那么K 1时刻的误差W3=sqrt(Y*Y V*V)=4.218。同理W1可以根据K-1时刻的相关量计算出来。

整个计算中误差与测量数据是独立变化的。最后附上一般形式卡尔曼滤波5大公式，找时间再彻底的推导一下。

​​