毫米波雷达笔记(2):什么是卡尔曼滤波?为什么使用EKF?EKF公式中Hk的推导


什么是卡尔曼滤波

假设想要得知k时刻的温度,可以通过经验进行预测,也可以用温度计进行测量,但是两者有误差(偏差),且假定误差符合高斯分布(与前后时间无关)。也就是说,预测值有高斯噪声,测量值也有高斯噪声,这2个噪声相互独立,单独的利用任何一个都不能很好的得到真实值,所以在2者之间有个信赖度的问题,应该相信谁更多些,这也就是卡尔曼算法的核心,这个信赖度就是卡尔曼增益,卡尔曼增益通过测量值和真实值之间的协方差最小时确定的,由此求这个协方差偏导为0时的系数,这个系数就是卡尔曼增益,这样就能很好的融合预测值和测量值。并且推导卡尔曼增益的时候,发现协方差是可以递归的,由此只要刚开始指定初始协方差就可以源源不断的求出卡尔曼增益和新的协方差,从而不断的跟新真实值。

卡尔曼滤波要做的,就是根据贝叶斯估计的相关理论,同时考虑预测模块和纠错模块的协方差,对误差小的项赋予较大的权重,对误差大的项赋予较小的权重,并使预测的误差最小。

养猪的案例【公式】

假设有一只猪,一周前的体重为46±0.5kg(表示猪的体重并不确定,46kg这个体重有0.5kg的误差);
一周后,根据多年经验的模型公式计算出的体重是48kg,误差是2kg;直接称体重,是49kg,误差是1kg;
我希望有一种方法,可以同时结合这只猪一周前的体重、用经验公式估计的值以及直接称量得到的值,综合考虑,得出一个最接近猪真实体重的,误差最小的值。这就是卡尔曼滤波要完成的任务。
将养猪的模型抽象为数学公式
在这里插入图片描述

上图中右边表示k时刻的状态值,这个值可以通过预测模块(也就是根据经验公式估计猪的体重)和纠错模块(也就是直接去称量猪的体重值)来估计。同样,预测模块和纠错模块都有着对应的误差和误差协方差矩阵。卡尔曼滤波要做的,就是根据贝叶斯估计的相关理论,同时考虑预测模块和纠错模块的协方差,对误差小的项赋予较大的权重,对误差大的项赋予较小的权重,并使预测的误差最小。
在这里插入图片描述

具体的实现过程如下:
在这里插入图片描述

跟踪坐标系的选用逻辑

一般来讲,在无杂波环境下,跟踪单个目标时,一般采用直角坐标系、NED坐标系或极坐标系;而在多回波环境下跟踪多个目标时,以及多平台目标跟踪时,采用混合坐标系较为方便。不管在何种情况下,所选的坐标系应满足:①易于目标的运动描述②满足滤波器的带宽要求③易于状态耦合和解耦④较小的动态和静态偏差⑤在满足跟踪精度的情况下减少计算量

为什么通常选用直角坐标系作为跟踪坐标系?

极坐标系下,测量方程为线性方程,状态方程为非线性方程且描述存在较大困难
直角坐标系下,状态方程为线性方程,测量方程是非线性方程【通过坐标转换法可以使之线性化】
在这里插入图片描述
在这里插入图片描述

为什么采用多种坐标系实现对目标的跟踪?

由于目标的运动方程在直角坐标系中可以用相对简单的状态方程足够精确地描述,因此,目标轨迹外推逻辑可以放在直角坐标系中完成;而目标残差、滤波增益、跟踪误差的协方差计算均在极坐标系中完成。——适合使用EKF滤波

在这里插入图片描述

EKF中的公式推导【含WIKI公式】

在这里插入图片描述

在这里插入图片描述
在这里插入图片描述
从wiki可知,EKF的公式为:
在这里插入图片描述

EKF公式中Hk的推导

在这里插入图片描述
在这里插入图片描述

参考资料

1、https://www.zhihu.com/question/23971601/answer/137325095
2、https://blog.csdn.net/czyv587/article/details/52061523
3、何友《雷达数据处理及应用第3版》
4、黄小平《卡尔曼滤波原理及应用——MATLAB仿真》

  • 6
    点赞
  • 24
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
无迹滤波(Unscented Kalman Filter,UKF)是一种非线性滤波算法,它通过一种称为“无迹变换”的方法,将非线性系统的状态估计问题转化为高斯分布的线性问题,从而避免了传统滤波需要进行数值积分的问题。相比于扩展滤波,无迹滤波更加准确,但是计算量也更大。 以下是一个简单的Python实现无迹滤波的例子: ```python import numpy as np from filterpy.kalman import UnscentedKalmanFilter as UKF # 定义状态转移函数 def fx(x, dt): return np.dot(F, x) # 定义观测函数 def hx(x): return np.dot(H, x) # 定义系统噪声和测量噪声 Q = np.diag([0.1, 0.1, 0.01, 0.01]) R = np.diag([0.1, 0.1]) # 定义初始状态和协方差矩阵 x0 = np.array([0., 0., 0., 0.]) P0 = np.diag([0.1, 0.1, 0.1, 0.1]) # 定义状态转移矩阵和观测矩阵 F = np.array([[1., 0., 1., 0.], [0., 1., 0., 1.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) H = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.]]) # 定义UKF滤波器 ukf = UKF(dim_x=4, dim_z=2, fx=fx, hx=hx, dt=0.1, points=UKF.sigma_points_fn(4, alpha=1e-3, beta=2., kappa=0.)) ukf.x = x0 ukf.P = P0 ukf.Q = Q ukf.R = R # 生成模拟数据 t = np.arange(0, 10, 0.1) x = np.vstack((np.sin(t), np.cos(t), 0.1*t, -0.1*t)) z = np.dot(H, x) + np.random.normal(size=(2, len(t))) # 进行滤波 for i in range(len(t)): ukf.predict() ukf.update(z[:, i]) # 输出结果 print("滤波后的状态估计值:", ukf.x) print("滤波后的状态协方差矩阵:", ukf.P) ```

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值