2018-06-10 py4fi(2)--卡尔曼滤波的实现（1）

卡尔曼的相关概念知识就不在本篇中复述了
第一篇主要提及一下几个核心的点和Python实现案例

工作流程如下图：

工作流程图

核心方程组：

先验估计方程.png
先验估计协方差矩阵.png
观测向量方程.png
卡尔曼增益.png
更新后的状态估计.png
更新后的协方差估计.png

Python实现案例：

卡尔玛滤波的实现
"""
# 这里是假设A=1，H=1的情况

# 参数初始化
n_iter = 50
sz = (n_iter,)  # size of array
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)  # a posteri estimate of x 滤波估计值
P = np.zeros(sz)  # a posteri error estimate滤波估计协方差矩阵
xhatminus = np.zeros(sz)  # a priori estimate of x 估计值
Pminus = np.zeros(sz)  # a priori error estimate估计协方差矩阵
K = np.zeros(sz)  # gain or blending factor卡尔曼增益

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, n_iter):
    # 预测
    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

    # 更新
    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

pylab.figure()
pylab.plot(z, 'k+', label='noisy measurements')  # 观测值
pylab.plot(xhat, 'b-', label='a posteri estimate')  # 滤波估计值
pylab.axhline(x, color='g', label='truth value')  # 真实值
pylab.legend()
pylab.xlabel('Iteration')
pylab.ylabel('Voltage')

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