文章目录
什么是卡尔曼滤波
假设想要得知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仿真》