kalman滤波

参见https://www.jianshu.com/p/d3b1c3d307e0
Kalman滤波算法的本质就是利用两个正态分布的融合仍是正态分布这一特性进行迭代而已

• 线性叠加定理，即位置和速度两正态分布的和也满足正态分布，方差是二者平方
• 两个高斯分布相乘仍然是高斯分布，这个结论不要求两变量不相关

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值：
Z(k)=H X(k)+V(k)
上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量，A和B是系统参数，对于多模型系统，它们为矩阵。而Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的协方差(covariance)分别是Q，R（这里我们假设他们不随系统状态变化而变化）。
卡尔曼滤波器5个公式：
X(k|k-1)=A X(k-1|k-1)+B U(k) ………… (7)
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (8)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))……… (9)
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)……… (10)
P(k|k)=（I-Kg(k) H）P(k|k-1) ………(11)

• 步骤一：

Kalman这个类需要初始化下面变量：
转移矩阵，测量矩阵，控制向量(没有的话，就是0)，过程噪声协方差矩阵，测量噪声协方差矩阵，后验错误协方差矩阵，前一状态校正后的值，当前观察值。

• 步骤二：

调用kalman这个类的predict方法得到状态的预测值矩阵，预测状态的计算公式如下：
predicted state (x’(k)):x’(k)=Ax(k-1)+Bu(k)
其中x(k-1)为前一状态的校正值，第一个循环中在初始化过程中已经给定了，后面的循环中Kalman这个类内部会计算。A,B,u(k),也都是给定了的值。这样进过计算就得到了系统状态的预测值x’(k)了。

• 步骤三：

调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵，其公式为：
corrected state (x(k)):x(k)=x’(k)+K(k)(z(k)-Hx’(k))
其中x’(k)为步骤二算出的结果，z(k)为当前测量值，是我们外部测量后输入的向量。H为Kalman类初始化给定的测量矩阵。K(k)为Kalman增益，其计算公式为：
Kalman gain matrix (K(k)): K(k)=P’(k)Htinv(H*P’(k)*Ht+R)
计算该增益所依赖的变量要么初始化中给定，要么在kalman理论中通过其它公式可以计算。
经过步骤三后，我们又重新获得了这一时刻的校正值，后面就不断循环步骤二和步骤三即可完成Kalman滤波过程。