日期：
来源：新机器视觉收集编辑：氢键H-H
点击下方卡片，关注“新机器视觉”公众号
重磅干货，第一时间送达
文章转载古月居
1. 卡尔曼滤波简介
2. 卡尔曼滤波原理
3. 卡尔曼滤波的公式
公式（二）：计算预测值和真实值之间的预测误差协方差矩阵
公式（三）：在公式一和公式二的条件下，求得卡尔曼增益
公式（四）：计算估计值
公式（五）：计算估计值和真实值之间的误差协方差矩阵
4. 卡尔曼滤波的公式推导
而上述的过程噪声ω 和 观测噪声υ皆服从高斯分布。
状态新息（状态 - 预测状态）：
5. 卡尔曼滤波应用
import numpy as npimport matplotlib.pyplot as pltdelta_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() # 显示图表
本文仅做学术分享，如有侵权，请联系删文。