一、基本原理

卡尔曼滤波分为两大阶段：预测和更新。

1.1 预测

预测公式如下：
x′=Fx+Bux^{\prime}=Fx+B u x′=Fx+Bu P′=FPFT+QP^{\prime}=F P F^{T}+Q P′=FPFT+Q
其中，x′x^{\prime}x′ 表示当前估计值；FFF 表示状态转移矩阵；xxx 表示上一时刻的最优估计值；BBB 表示外部输入矩阵；UUU 表示外部状态矩阵；P′P^{\prime}P′ 表示当前的先验估计协方差矩阵；PPP 表示上一时刻的最优估计协方差矩阵；QQQ 表示过程的协方差矩阵。

1.2 更新

1.2.1 写法一

更新公式如下：
K=P′HT(HP′HT+R)−1K=P^{\prime} H^{T}\left(H P^{\prime} H^{T}+R\right)^{-1} K=P′HT(HP′HT+R)−1x=x′+K(z−Hx′)x=x^{\prime}+K\left(z-H x^{\prime}\right) x=x′+K(z−Hx′)P=(I−KH)P′P=(I-K H) P^{\prime} P=(I−KH)P′
其中，KKK 表示当前时刻的卡尔曼增益；HHH 表示观测矩阵；xxx 表示根据当前时刻的估计值及观测值融合得到的当前时刻的最优估计值；zzz 表示实际测量值；PPP 表示当前时刻的协方差矩阵；III 表示单位矩阵。

1.2.2 写法二

更新公式如下：
y=z−Hx′y=z-H x^{\prime} y=z−Hx′S=HP′HT+RS=H P^{\prime} H^{T}+R S=HP′HT+RK=P′HTS−1K=P^{\prime} H^{T} S^{-1} K=P′HTS−1x=x′+Kyx=x^{\prime}+K y x=x′+KyP=(I−KH)P′P=(I-K H) P^{\prime} P=(I−KH)P′
其中，yyy 和 SSS 无实际含义，属于中间变量；KKK 表示当前时刻的卡尔曼增益；HHH 表示观测矩阵；xxx 表示根据当前时刻的估计值及观测值融合得到的当前时刻的最优估计值；zzz 表示实际测量值；PPP 表示当前时刻的协方差矩阵；III 表示单位矩阵。

二、举例

本例子采用的是更新写法二
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.0last_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]])      # 控制输入矩阵BB = 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 = 0acceleration_y_posterior_est = 0else:acceleration_x_posterior_est = (speed_x_posterior_est[i-3] - speed_x_posterior_est[i-4])/delta_tacceleration_y_posterior_est = (speed_y_posterior_est[i-3] - speed_y_posterior_est[i-4])/delta_t# 控制状态矩阵UU = 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]]) # 计算状态估计协方差矩阵PP_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]])# 计算卡尔曼增益Kk1 = 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。

四、小贴士

计算外部状态UUU的时候，不能用预测值来计算，得用估计值。

参考资料

卡尔曼滤波简介
快速上手的Python版二维卡尔曼滤波解析
多传感器融合定位1（激光雷达+毫米波雷达）
使用卡尔曼滤波和扩展卡尔曼滤波进行毫米波雷达和激光雷达数据融合示例

卡尔曼滤波算法——基本原理及举例（python实现radar数据滤波）相关推荐

滤波系列（一）卡尔曼滤波算法（KF）：详细数学推导
滤波系列(一)卡尔曼滤波算法(KF) 在本文,将给出卡尔曼滤波算法的详细数学推导过程,如果想直接了解卡尔曼滤波算法的应用,请看博客:卡尔曼滤波算法的应用(python代码)或者直接可以调用Python ...
卡尔曼滤波与组合导航原理_卫星知识科普：一种基于卫星共视的卡尔曼滤波算法！...
如果看到第二次了,该关注我们了,点击上面关注,分享IT技术知识. 1 ,概述共视法是利用2个不同地点观测站的卫星接收机同时跟踪同一颗卫星,从而降低2站间共同误差,提高时间同步精度的方法.高精度的共视 ...
卡尔曼滤波算法的代码验证
卡尔曼滤波算法它能够从一系列的不完全及包含噪声的测量中,估计动态系统的状态,实践中得到广泛应用,场景包括控制系统参数估计,移动预测等等,在深度学习中也常常用来配合神经网络输出做后处理,应用于目标跟踪等 ...
python实现排列组合公式算法_Python实现卡尔曼滤波算法之贝叶斯滤波
Python实现卡尔曼滤波算法之贝叶斯滤波作者:yangjian 卡尔曼滤波器属于贝叶斯滤波器的一种特例,本文主要讲解贝叶斯滤波原理及其算法的python实现. 先来看下贝叶斯公式贝叶斯公式 :后 ...
python中值滤波算法_Python实现卡尔曼滤波算法之贝叶斯滤波
Python实现卡尔曼滤波算法之贝叶斯滤波作者:yangjian 卡尔曼滤波器属于贝叶斯滤波器的一种特例,本文主要讲解贝叶斯滤波原理及其算法的python实现. 先来看下贝叶斯公式贝叶斯公式 :后 ...
python卡尔曼滤波室内定位_基于扩展卡尔曼滤波算法的室内定位跟踪系统
基于扩展卡尔曼滤波算法的室内定位跟踪系统凌海波,周先存 [摘要] 摘要:为了解决无线室内定位系统实时跟踪位置坐标误差较大问题, 提出一种基于扩展卡尔曼滤波 (EKF) 算法的室内定位方法.系统采用 ...
解读基于多传感器融合的卡尔曼滤波算法
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自|3D视觉工坊卡尔曼滤波器是传感器融合工程师用于自动驾驶 ...
卡尔曼滤波算法原理(KF,EKF,AKF,UKF)
卡尔曼滤波算法原理(KF,EKF,AKF,UKF) 主要是KF.EKF.UKF算法公式推导,直接看公式会比较枯燥,建议推导一下. 新增文章卡尔曼运动模型公式推导CTRV+CTRA,主要是EKF的CTR ...
python算法与程序设计基础第二版-算法与程序设计基础(Python版) - 吴萍
基本信息书名:21世纪高等学校计算机基础实用规划教材:算法与程序设计基础(Python版) 定价:39.00元作者:吴萍21世纪高出版社:清华大学出版社出版日期:2015_2_1 ISBN:9 ...

卡尔曼滤波算法——基本原理及举例（python实现radar数据滤波）