Kalman filtering

卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。

what? 最小均方误差不就是最小二乘吗？

模型的预测+测量的反馈（权重由kalman gain决定）形成新的高斯分布，所以可以作为下次迭代的起始点。

PRML Chapter13 线性动态系统从概率的角度讲叙了kalman Filter 。或者参考卡尔曼滤波器学习笔记（一）。

附截图

5条黄金公式的推导：####（参考卡尔曼滤波 -- 从推导到应用(一)，自己再推导了一遍）

最后盗一张算法流程图：

概率机器人——贝叶斯滤波

通俗地解释卡尔曼滤波器（一）——从贝叶斯滤波器说起

# http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt

#这里是假设A=1，H=1的情况

iterTimes = 50
sz = (iterTimes,)
x = -0.37727  # truth value (typo in example at top of p. 13 calls this z)
z = np.random.normal(x,0.1,size=sz)  # observations (normal about x, sigma=0.1)

Q = 1e-5  # process variance

xhat = np.zeros(sz)
P = np.zeros(sz)
xhatminus = np.zeros(sz)
Pminus=np.zeros(sz)
K=np.zeros(sz)

R = 0.1**2 # estimate of measurement variance, change to see effect

# intial guesses
xhat[0] = 0.0
P[0] = 1.0

for k in range(1,iterTimes):
    # time update
    xhatminus[k] = xhat[k-1]  #X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0
    Pminus[k] = P[k-1]+Q      #P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1

    # measurement update
    K[k] = Pminus[k]/( Pminus[k]+R ) #Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
    xhat[k] = xhatminus[k]+K[k]*(z[k]-xhatminus[k]) #X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
    P[k] = (1-K[k])*Pminus[k] #P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1

plt.figure(num=1,figsize=(8,6))
plt.xlabel("Iteration",size=14)
plt.ylabel("Voltage",size=14)
plt.plot(z,'k+',label='noisy measurements')     #测量值
plt.plot(xhat,'b-',label='a posteri estimate')  #过滤后的值
plt.axhline(x,color='g',label='truth value')    #系统值
plt.legend()
plt.savefig('kf.png',format='png')

plt.figure()
valid_iter = range(1,iterTimes) # Pminus not valid at step 0
plt.plot(valid_iter,Pminus[valid_iter],label='a priori error estimate')
plt.xlabel('Iteration')
plt.ylabel('$(Voltage)^2$')
plt.setp(plt.gca(),'ylim',[0,.01])
plt.savefig('kf1.png',format='png')