拓展卡尔曼滤波学习（python源码）

拓展卡尔曼滤波的逐步理解与实现

这个文章讲的非常不错。配套代码实现文章。

【机器人位置估计】卡尔曼滤波的原理与实现

本文主要是针对两篇文章的基础上做笔记和记录学习过程。

1、基本模型

1.1 机器人小M

现在小M只具有一个物理量-位移x，也就是一维卡尔曼

1.2 位移状态预测值

估计值自身会由于运动模型预测不准确而导致预测误差，由误差值得到的状态值也是存在误差的，如果以存在误差的状态值继续预测下一个状态值，误差会存在累计的问题。需要引入测量量进行修正。

1.3 传感器测量值

同样，我们使用的传感器也是存在测量精度的问题，无法完全的等于实际物理量的大小。

所以也就提出了卡尔曼老人解决这个问题的思路。既然测量值和估计值都存在一定的误差值，那么将两组数据进行融合，谁更准便将其权重提高。

1.4 卡尔曼核心思想

确定那个数据的置信度更高，这就牵扯到了预测误差的准确值Pt，所以需要通过一定的方式对Pt进行计算，随着整个过程进行一定的调整和变化。

1.5 计算 预测误差准确值

没有绝对准确的运动模型，也没有绝对准确的传感器

1.6 卡尔曼滤波公式

预测就是利用上一步的状态准确值和预测误差准确值，结合运动模型得到下一步的状态估计值和预测误差估计值。

更新就是融入传感器信息，计算增益系数，将估计值再次转换为准确值。

2、拓展卡尔曼滤波

2.1 加入控制量

2.2 观测系数

这里给的例子很明确的表现观测系数c的作用。引入观测洗漱势必会对预测误差产生一定比例的影响，发生一定变化。

2.3 拓展卡尔曼滤波公式

多维卡尔曼滤波

3.使用实例

import numpy as np
import math
import matplotlib.pyplot as plt
 
if __name__=="__main__":
    ## 1.设计一个匀加速直线运动，以观测此运动
    X_real = np.mat(np.zeros((2, 100))) # 空矩阵，用于存放真实状态向量
    X_real[:, 0] = np.mat([[0.0], # 初始状态向量
                           [1.0]])
    a_real = 0.1# 真实加速度
    F = np.mat([[1.0, 1.0], # 状态转移矩阵
                [0.0, 1.0]])
    Q = np.mat([[0.0001, 0.0], # 状态转移协方差矩阵，我们假设外部干扰很小
                [0.0, 0.0001]])
    B = np.mat([[0.5], # 控制矩阵
                [1.0]])
    for i in range(99):
        X_real[:, i + 1] = F * X_real[:, i] + B * a_real # 计算真实状态向量
    X_real = np.array(X_real)
    fig = plt.figure(1)
    plt.grid()
    plt.title('real displacement')
    plt.xlabel('k (s)')
    plt.ylabel('x (m)')
    plt.plot(X_real[0, :])
    plt.show()
    fig = plt.figure(2)
    plt.grid()
    plt.title('real velocity')
    plt.xlabel('k (s)')
    plt.ylabel('v (m/s)')
    plt.plot(X_real[1, :])
    plt.show()
    X_real = np.mat(X_real)

    ## 2.建立传感器观测值
    z_t = np.mat(np.zeros((2, 100))) # 空矩阵，用于存放传感器观测值
    H = np.mat(np.zeros((2, 2)))
    H[0, 0], H[1, 1] = -1.0, -1.0
    noise = np.mat(np.random.randn(2,100)) # 加入位移方差为1，速度方差为1的传感器噪声
    R = np.mat([[1.0, 0.0], # 观测噪声的协方差矩阵
                [0.0, 1.0]])
    for i in range(100):
        z_t[:, i] = H * X_real[:, i] + noise[:, i]
    z_t = np.array(z_t)
    fig = plt.figure(3)
    plt.grid()
    plt.title('sensor displacement')
    plt.xlabel('k (s)')
    plt.ylabel('x (m)')
    plt.plot(z_t[0, :])
    plt.show()
    fig = plt.figure(4)
    plt.grid()
    plt.title('sensor velocity')
    plt.xlabel('k (s)')
    plt.ylabel('v (m/s)')
    plt.plot(z_t[1, :])
    plt.show()
    z_t = np.mat(z_t)

卡尔曼滤波实现

    ## 3.执行线性卡尔曼滤波
    Q = np.mat([[1.0, 0.0], # 状态转移协方差矩阵，我们假设外部干扰很小，
                [0.0, 1.0]])# 转移矩阵可信度很高
    # 建立一系列空序列用于储存结果
    X_update = np.mat(np.zeros((2, 100)))
    P_update = np.zeros((100, 2, 2))
    X_predict = np.mat(np.zeros((2, 100)))
    P_predict = np.zeros((100, 2, 2))
    P_update[0, :, :] = np.mat([[1.0, 0.0], # 状态向量协方差矩阵初值
                                [0.0, 1.0]])
    P_predict[0, :, :] = np.mat([[1.0, 0.0], # 状态向量协方差矩阵初值
                                 [0.0, 1.0]])
    for i in range(99):
        # 预测
        X_predict[:, i + 1] = F * X_update[:, i] + B * a_real
        P_p = F * np.mat(P_update[i, :, :]) * F.T + Q
        P_predict[i + 1, :, :] = P_p
        # 更新
        K = P_p * H.T * np.linalg.inv(H * P_p * H.T + R) # 卡尔曼增益
        P_u = P_p - K * H * P_p
        P_update[i + 1, :, :] = P_u
        X_update[:, i + 1] = X_predict[:, i + 1] + K * (z_t[:, i + 1] - H * X_predict[:, i + 1])
    X_update = np.array(X_update)   
    X_real = np.array(X_real) 
    fig = plt.figure(5)
    plt.grid()
    plt.title('Kalman predict displacement')
    plt.xlabel('k (s)')
    plt.ylabel('x (m)')
    plt.plot(X_real[0, :], label='real', color='b')
    plt.plot(X_update[0, :], label='predict', color='r')
    plt.legend()
    plt.show()
    fig = plt.figure(6)
    plt.grid()
    plt.title('Kalman predict velocity')
    plt.xlabel('k (s)')
    plt.ylabel('v (m/s)')
    plt.plot(X_real[1, :], label='real', color='b')
    plt.plot(X_update[1, :], label='predict', color='r')
    plt.legend()
    plt.show()
    X_update = np.mat(X_update)
    X_real = np.mat(X_real)

这里kt取到了很低，非常不信任传感器测量到的值。

卡尔曼滤波将存在误差的传感器信号和状态向量很好的融合，较好地预测出了真实地机器人运动状态。