卡尔曼滤波原理与实践（python）

简述：

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

预备内容：

方差：各个样本与样本均值的差的平方和的均值，度量一组数据的分散程度。 协方差：两个变量总体误差的期望，度量两个变量线性相关性程度。 当两个变量是相同的情况，协方差就是方差。 如果两个变量的变化趋势相反，其中一个变量大于自身的期望值时，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。 两者之间的联系或者关系，关系越大，协方差越大。 协方差矩阵：数据集中两两变量的协方差组成，每个元素是各个向量元素之间的协方差 噪声协方差矩阵越小说明噪声的误差越小，可信度越高，其对角线上的值就是方差。 误差协方差矩阵越小说明过程噪声和量测噪声的关系越小，用比例分开过程噪声ω 和 观测噪声υ，如果关系越小，分开的越精确。比如一堆白砂糖和盐，如果两种混合的很均匀，说它关系很大，也就越难用比例的方法将其分开。
协方差学习链接

线性系统

模型，运动方程和观测方程

其中，A为状态转移矩阵，B为可控的输入增益；W为运动模型系统的噪声（如导航系统里程计的测量噪声），V为观测系统噪声（如导航系统的相机观测噪声）；
其中这两部分噪声符合高斯分布：

预测和更新

注： xk表示K时刻的状态， xk^帽子表示K时刻的优化预测状态（非真实状态）； 而xk-表示K时刻的预测状态，该状态（先验）依据上一个状态的后验得来，之后在更新部分进行修正去掉-。其中 p-表示协方差矩阵（误差矩阵），当估计状态是1维数据时，若加入噪声，仅考虑方差 sita就可以；当估计状态是高维时，为了考虑各个维度偏离均值的程度，必须引入协方差矩阵sigma 。Kk为卡尔曼增益（观测权重）；H为观测噪声；Q预测噪声协方差；R观测噪声协方差。

公式推导此处不展示，有兴趣这可参考文章底部资料了解（免费）

非线性系统：

模型，运动模型和观测方程

线性化（Taylor Series）
在x0处泰勒一阶展开

预测和更新：

总体流程

实践部分

code：

import numpy as np
import matplotlib.pyplot as plt

delta_t = 0.1                               # 每秒钟采一次样
end_t = 7                                   # 时间长度
time_t = end_t * 10                         # 采样次数
t = np.arange(0, end_t, delta_t)            # 设置时间数组
u = 1                                       # 定义外界对系统的作用 加速度
x = 1 / 2 * u * t ** 2                      # 实际真实位置

v_var = 1                                   # 测量噪声的方差
# 创建高斯噪声，精确到小数点后两位
v_noise = np.round(np.random.normal(0, v_var, time_t), 2)

X = np.mat([[0], [0]])                      # 定义预测优化值的初始状态
v = np.mat(v_noise)                         # 定义测量噪声
z = x + v                                   # 定义测量值（假设测量值=实际状态值+噪声）
A = np.mat([[1, delta_t], [0, 1]])          # 定义状态转移矩阵
B = [[1 / 2 * (delta_t ** 2)], [delta_t]]   # 定义输入控制矩阵
P = np.mat([[1, 0], [0, 1]])                # 定义初始状态协方差矩阵
Q = np.mat([[0.001, 0], [0, 0.001]])        # 定义状态转移(预测噪声)协方差矩阵
H = np.mat([1, 0])                          # 定义观测矩阵
R = np.mat([1])                             # 定义观测噪声协方差
X_mat = np.zeros(time_t)                    # 初始化记录系统预测优化值的列表

for i in range(time_t):
    # 预测
    X_predict = A * X + np.dot(B, u)        # 估算状态变量
    P_predict = A * P * A.T + Q             # 估算状态误差协方差
    # 校正
    K = P_predict * H.T / (H * P_predict * H.T + R)     # 更新卡尔曼增益
    X = X_predict + K * (z[0, i] - H * X_predict)       # 更新预测优化值
    P = (np.eye(2) - K * H) * P_predict                 # 更新状态误差协方差
    # 记录系统的预测优化值
    X_mat[i] = X[0, 0]

plt.rcParams['font.sans-serif'] = ['SimHei']    # 设置正常显示中文
plt.plot(x, "b", label='实际状态值')             # 设置曲线数值
plt.plot(X_mat, "g", label='预测优化值')
plt.plot(z.T, "r--", label='测量值')
plt.xlabel("时间")                               # 设置X轴的名字
plt.ylabel("位移")                               # 设置Y轴的名字
plt.title("卡尔曼滤波示意图")                     # 设置标题
plt.legend()                                    # 设置图例
plt.show()                                      # 显示图表

注释详细，不做解释，对照上边五个公式。
运行结果：

理论推导以及python应用可参考（较为专业资源）：
https://download.csdn.net/download/weixin_45080292/85418585
优秀博客推荐：
https://medium.com/@jaems33/understanding-kalman-filters-with-python-2310e87b8f48