拓展卡尔曼滤波学习（python源码）
程序员文章站
2022-07-14 22:24:56
...
这个文章讲的非常不错。配套代码实现文章。
本文主要是针对两篇文章的基础上做笔记和记录学习过程。
一、基本模型
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取到了很低，非常不信任传感器测量到的值。
卡尔曼滤波将存在误差的传感器信号和状态向量很好的融合，较好地预测出了真实地机器人运动状态。