卡尔曼滤波算法——基本原理及举例（python实现radar数据滤波）
卡尔曼滤波分为两大阶段：预测和更新。预测公式如下：x′=Fx+Bux^{\prime}=Fx+B ux′=Fx+Bu P′=FPFT+QP^{\prime}=F P F^{T}+QP′=FPFT+Q其中，x′x^{\prime}x′ 表示；FFF 表示；xxx 表示；BBB 表示；UUU 表示；P′P^{\prime}P′ 表示；PPP 表示；QQQ 表示。更新公式如下：K=P′HT(HP′HT+
·
卡尔曼滤波算法——基本原理及举例（python实现radar数据滤波）
一、基本原理
卡尔曼滤波分为两大阶段：预测和更新。
1.1 预测
预测公式如下：
x ′ = F x + B u x^{\prime}=Fx+B u x′=Fx+Bu P ′ = F P F T + Q P^{\prime}=F P F^{T}+Q P′=FPFT+Q
其中， x ′ x^{\prime} x′ 表示当前估计值； F F F 表示状态转移矩阵； x x x 表示上一时刻的最优估计值； B B B 表示外部输入矩阵； U U U 表示外部状态矩阵； P ′ P^{\prime} P′ 表示当前的先验估计协方差矩阵； P P P 表示上一时刻的最优估计协方差矩阵； Q Q Q 表示过程的协方差矩阵。
1.2 更新
1.2.1 写法一
更新公式如下：
K = P ′ H T ( H P ′ H T + R ) − 1 K=P^{\prime} H^{T}\left(H P^{\prime} H^{T}+R\right)^{-1} K=P′HT(HP′HT+R)−1 x = x ′ + K ( z − H x ′ ) x=x^{\prime}+K\left(z-H x^{\prime}\right) x=x′+K(z−Hx′) P = ( I − K H ) P ′ P=(I-K H) P^{\prime} P=(I−KH)P′
其中， K K K 表示当前时刻的卡尔曼增益； H H H 表示观测矩阵； x x x 表示根据当前时刻的估计值及观测值融合得到的当前时刻的最优估计值； z z z 表示实际测量值； P P P 表示当前时刻的协方差矩阵； I I I 表示单位矩阵。
1.2.2 写法二
更新公式如下：
y = z − H x ′ y=z-H x^{\prime} y=z−Hx′ S = H P ′ H T + R S=H P^{\prime} H^{T}+R S=HP′HT+R K = P ′ H T S − 1 K=P^{\prime} H^{T} S^{-1} K=P′HTS−1 x = x ′ + K y x=x^{\prime}+K y x=x′+Ky P = ( I − K H ) P ′ P=(I-K H) P^{\prime} P=(I−KH)P′
其中， y y y 和 S S S 无实际含义，属于中间变量； K K K 表示当前时刻的卡尔曼增益； H H H 表示观测矩阵； x x x 表示根据当前时刻的估计值及观测值融合得到的当前时刻的最优估计值； z z z 表示实际测量值； P P P 表示当前时刻的协方差矩阵； I I I 表示单位矩阵。
二、举例
本例子采用的是更新写法二
radar测量如下：
2.1 数据说明
2.2 代码
# 功能说明：
# 利用卡尔曼滤波来对radar数据滤波
import numpy as np
import matplotlib.pyplot as plt
from math import sin,cos,sqrt # sin,cos的输入是 弧度
if __name__ == "__main__":
file = open('sample-laser-radar-measurement-data-2.txt')
# 数据变量初始化
position_rho_measure = []
position_theta_measure = []
position_velocity_measure = []
time_measure = []
position_x_true = []
position_y_true = []
speed_x_true = []
speed_y_true = []
# x,y方向的测量值(由非线性空间转到线性空间)
position_x_measure = []
position_y_measure = []
speed_x_measure = []
speed_y_measure = []
# 先验估计值
position_x_prior_est = [] # x方向位置的先验估计值
position_y_prior_est = [] # y方向位置的先验估计值
speed_x_prior_est = [] # x方向速度的先验估计值
speed_y_prior_est = [] # y方向速度的先验估计值
# 估计值和观测值融合后的最优估计值
position_x_posterior_est = [] # 根据估计值及当前时刻的观测值融合到一体得到的最优估计值x位置值存入到列表中
position_y_posterior_est = [] # 根据估计值及当前时刻的观测值融合到一体得到的最优估计值y位置值存入到列表中
speed_x_posterior_est = [] # 根据估计值及当前时刻的观测值融合到一体得到的最优估计x速度值存入到列表中
speed_y_posterior_est = [] # 根据估计值及当前时刻的观测值融合到一体得到的最优估计y速度值存入到列表中
# 读取radar数据
for line in file.readlines():
curLine = line.strip().split(" ")
# 取出radar数据
if curLine[0] == 'R':
position_rho_measure.append(float(curLine[1]))
position_theta_measure.append(float(curLine[2]))
position_velocity_measure.append(float(curLine[3]))
time_measure.append(float(curLine[4]))
position_x_true.append(float(curLine[5]))
position_x_true.append(float(curLine[6]))
speed_x_true.append(float(curLine[7]))
speed_y_true.append(float(curLine[8]))
# 测量值 由非线性空间转到线性空间
position_x_measure.append(float(curLine[1])*cos(float(curLine[2])))
position_y_measure.append(float(curLine[1])*sin(float(curLine[2])))
speed_x_measure.append(float(curLine[3])*cos(float(curLine[2])))
speed_y_measure.append(float(curLine[3])*sin(float(curLine[2])))
# --------------------------- 初始化 -------------------------
# 用第二帧测量数据初始化
X0 = np.array([[position_x_measure[1]],[position_y_measure[1]],[speed_x_measure[1]],[speed_y_measure[1]]])
# 用第二帧初始时间戳
last_timestamp_ = time_measure[1]
# 状态估计协方差矩阵P初始化（其实就是初始化最优解的协方差矩阵）
P = np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
X_posterior = np.array(X0) # X_posterior表示上一时刻的最优估计值
P_posterior = np.array(P) # P_posterior是继续更新最优解的协方差矩阵
# 将初始化后的数据依次送入(即从第三帧速度往里送)
for i in range(2,len(time_measure)):
# ------------------- 下面开始进行预测和更新，来回不断的迭代 -------------------------
# 求前后两帧的时间差，数据包中的时间戳单位为微秒，处以1e6，转换为秒
delta_t = (time_measure[i] - last_timestamp_) / 1000000.0
last_timestamp_ = time_measure[i]
# print("delta_t",delta_t)
# 状态转移矩阵F，上一时刻的状态转移到当前时刻
F = np.array([[1.0, 0.0, delta_t, 0.0],
[0.0, 1.0, 0.0, delta_t],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
# 控制输入矩阵B
B = np.array([[delta_t*delta_t/2.0, 0.0],
[0.0, delta_t*delta_t/2.0],
[delta_t, 0.0],
[0.0, delta_t]])
# 计算加速度也需要用估计的速度来做，而不是测量的速度
# i = 4 开始，速度预测列表才会有2个值及以上
if i == 2 or i == 3:
acceleration_x_posterior_est = 0
acceleration_y_posterior_est = 0
else:
acceleration_x_posterior_est = (speed_x_posterior_est[i-3] - speed_x_posterior_est[i-4])/delta_t
acceleration_y_posterior_est = (speed_y_posterior_est[i-3] - speed_y_posterior_est[i-4])/delta_t
# 控制状态矩阵U
U = np.array([[acceleration_x_posterior_est],[acceleration_y_posterior_est]])
# 打印测试
print("acceleration_x_posterior_est",acceleration_x_posterior_est)
print("acceleration_y_posterior_est",acceleration_y_posterior_est)
# ---------------------- 预测 -------------------------
# X_prior = np.dot(F,X_posterior) + np.dot(B,U) # 使用加速度，X_prior表示根据上一时刻的最优估计值得到当前的估计值 X_posterior表示上一时刻的最优估计值
X_prior = np.dot(F,X_posterior) # 不使用加速度，X_prior表示根据上一时刻的最优估计值得到当前的估计值 X_posterior表示上一时刻的最优估计值
position_x_prior_est.append(X_prior[0,0]) # 将根据上一时刻计算得到的x方向最优估计位置值添加到列表position_x_prior_est中
position_y_prior_est.append(X_prior[1,0]) # 将根据上一时刻计算得到的y方向最优估计位置值添加到列表position_y_prior_est中
speed_x_prior_est.append(X_prior[2,0]) # 将根据上一时刻计算得到的x方向最优估计速度值添加到列表speed_x_prior_est中
speed_y_prior_est.append(X_prior[3,0]) # 将根据上一时刻计算得到的x方向最优估计速度值添加到列表speed_y_prior_est中
# Q:过程噪声的协方差，p(w)~N(0,Q)，噪声来自真实世界中的不确定性，N(0,Q) 表示期望是0，协方差矩阵是Q。Q中的值越小，说明预估的越准确。
Q = np.array([[0.0001, 0.0, 0.0, 0.0],
[0.0, 0.00009, 0.0, 0.0],
[0.0, 0.0, 0.001, 0.0],
[0.0, 0.0, 0.0, 0.001]])
# 计算状态估计协方差矩阵P
P_prior_1 = np.dot(F,P_posterior) # P_posterior是上一时刻最优估计的协方差矩阵 # P_prior_1就为公式中的（F.Pk-1）
P_prior = np.dot(P_prior_1, F.T) + Q # P_prior是得出当前的先验估计协方差矩阵 # Q是过程协方差
# ------------------- 更新 ------------------------
# 测量的协方差矩阵R，一般厂家给提供，R中的值越小，说明测量的越准确。
R = np.array([[0.009, 0.0, 0.0, 0.0],
[0.0, 0.0009, 0.0, 0.0],
[0.0, 0.0, 9, 0.0],
[0.0, 0.0, 0, 9]])
# 状态观测矩阵H（KF,radar,4*4）
H = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
# 计算卡尔曼增益K
k1 = np.dot(P_prior, H.T) # P_prior是得出当前的先验估计协方差矩阵
k2 = np.dot(np.dot(H, P_prior), H.T) + R # R是测量的协方差矩阵
K = np.dot(k1, np.linalg.inv(k2)) # np.linalg.inv()：矩阵求逆 # K就是当前时刻的卡尔曼增益
# 测量值（数据输入的时候，就进行了线性化处理，从而可以使用线性H矩阵）
Z_measure = np.array([[position_x_measure[i]],[position_y_measure[i]],[speed_x_measure[i]],[speed_y_measure[i]]])
X_posterior_1 = Z_measure - np.dot(H, X_prior) # X_prior表示根据上一时刻的最优估计值得到当前的估计值
X_posterior = X_prior + np.dot(K, X_posterior_1) # X_posterior是根据估计值及当前时刻的观测值融合到一体得到的最优估计值
position_x_posterior_est.append(X_posterior[0, 0]) # 根据估计值及当前时刻的观测值融合到一体得到的最优估计x位置值存入到列表中
position_y_posterior_est.append(X_posterior[1, 0]) # 根据估计值及当前时刻的观测值融合到一体得到的最优估计y位置值存入到列表中
speed_x_posterior_est.append(X_posterior[2, 0]) # 根据估计值及当前时刻的观测值融合到一体得到的最优估计x速度值存入到列表中
speed_y_posterior_est.append(X_posterior[3, 0]) # 根据估计值及当前时刻的观测值融合到一体得到的最优估计y速度值存入到列表中
# 更新状态估计协方差矩阵P （其实就是继续更新最优解的协方差矩阵）
P_posterior_1 = np.eye(4) - np.dot(K, H) # np.eye(4)返回一个4维数组，对角线上为1，其他地方为0，其实就是一个单位矩阵
P_posterior = np.dot(P_posterior_1, P_prior) # P_posterior是继续更新最优解的协方差矩阵 # P_prior是得出的当前的先验估计协方差矩阵
# 可视化显示
if True:
plt.rcParams['font.sans-serif'] = ['SimHei'] # 坐标图像中显示中文
plt.rcParams['axes.unicode_minus'] = False
# 一、绘制x-y图
plt.xlabel("x")
plt.ylabel("y")
plt.plot(position_x_posterior_est, position_y_posterior_est, color='red', label="卡尔曼滤波后的值")
# plt.plot(position_x_true, position_y_true, color='green', label="真实值")
plt.scatter(position_x_measure, position_y_measure, color='blue', label="测量值")
plt.legend() # Add a legend.
plt.show()
# 二、单独绘制x,y,Vx,Vy图像
# fig, axs = plt.subplots(2, 2)
# # axs[0,0].plot(position_x_true, "-", label="位置x_实际值", linewidth=1)
# axs[0,0].plot(position_x_measure, "-", label="位置x_测量值", linewidth=1)
# axs[0,0].plot(position_x_posterior_est, "-", label="位置x_卡尔曼滤波后的值(融合测量值和估计值)", linewidth=1)
# axs[0,0].set_title("位置x")
# axs[0,0].set_xlabel('k')
# axs[0,0].legend()
# # axs[0,1].plot(position_y_true, "-", label="位置y_实际值", linewidth=1)
# axs[0,1].plot(position_y_measure, "-", label="位置y_测量值", linewidth=1)
# axs[0,1].plot(position_y_posterior_est, "-", label="位置y_卡尔曼滤波后的值(融合测量值和估计值)", linewidth=1)
# axs[0,1].set_title("位置y")
# axs[0,1].set_xlabel('k')
# axs[0,1].legend()
# # axs[1,0].plot(speed_x_true, "-", label="速度x_实际值", linewidth=1)
# axs[1,0].plot(speed_x_measure, "-", label="速度x_测量值", linewidth=1)
# axs[1,0].plot(speed_x_posterior_est, "-", label="速度x_卡尔曼滤波后的值(融合测量值和估计值)", linewidth=1)
# axs[1,0].set_title("速度x")
# axs[1,0].set_xlabel('k')
# axs[1,0].legend()
# # axs[1,1].plot(speed_y_true, "-", label="速度y_实际值", linewidth=1)
# axs[1,1].plot(speed_y_measure, "-", label="速度y_测量值", linewidth=1)
# axs[1,1].plot(speed_y_posterior_est, "-", label="速度y_卡尔曼滤波后的值(融合测量值和估计值)", linewidth=1)
# axs[1,1].set_title("速度y")
# axs[1,1].set_xlabel('k')
# axs[1,1].legend()
# plt.show()
2.3 实验结果
注明：
- 图中的“扩展”两个字需删除。
- 图中测量值和预测值横坐标尚未对齐。测量值的前两个值因未参与预测，故需要舍去，这样绘图才能保证对齐。
三、调参技巧
- 一般来说，上一时刻数据与这一时刻数据的间隔时间一般是已知的，若不已知，则需要想办法得到间隔时间。（不同的间隔时间，对后续的其他参数有影响）
- 调参主要调协方差矩阵P的初始值、过程的协方差矩阵Q、测量的协方差矩阵R。
- 先调好测量的协方差矩阵R，R一般是厂家给出的，其数值越小，代表测量精度越高。
- 再调调协方差矩阵P的初始值。
- 最后调过程的协方差矩阵Q。
四、小贴士
- 计算外部状态 U U U的时候，不能用预测值来计算，得用估计值。
参考资料
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
10
0- 0
已为社区贡献6条内容
所有评论(0)