卡尔曼滤波系列 |
您所在的位置:网站首页 › 求雅可比矩阵的方法有哪些 › 卡尔曼滤波系列 |
更新日志: 2020.02.13:修改了第三节推导中的公式错误 2020.03.21:修改了2.1节中的部分表述和公式加粗,补充迹的求导公式 2021.04.14:修改公式显示错误 1 简介扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。 EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用卡尔曼滤波框架对信号进行滤波,因此它是一种次优滤波。 2 算法介绍 2.1 泰勒级数展开泰勒级数展开是将一个在处具有阶导数的函数,利用关于的次多项式逼近函数值的方法。 若函数在包含的某个闭区间上具有阶导数,且在开区间上具有阶导数,则对闭区间上的任意一点,都有: 其中表示函数在处的阶导数,等式右边成为泰勒展开式,剩余的是泰勒展开式的余项,是的高阶无穷小。 (著名的欧拉公式就是利用,和的泰勒展开式得来的!) 当变量是多维向量时,一维的泰勒展开就需要做拓展,具体形式如下: 其中,表示雅克比矩阵,表示高阶无穷小。 这里,为维,状态向量为维,。 一般来说,EKF在对非线性函数做泰勒展开时,只取到一阶导和二阶导,而由于二阶导的计算复杂性,更多的实际应用只取到一阶导,同样也能有较好的结果。取一阶导时,状态转移方程和观测方程就近似为线性方程,高斯分布的变量经过线性变换之后仍然是高斯分布,这样就能够延用标准卡尔曼滤波的框架。 2.1 EKF标准卡尔曼滤波KF的状态转移方程和观测方程为 扩展卡尔曼滤波EKF的状态转移方程和观测方程为 (1) (2) 利用泰勒展开式对(1)式在上一次的估计值处展开得 (3) 再利用泰勒展开式对(2)式在本轮的状态预测值处展开得 (4) 其中,和分别表示函数和在和处的雅克比矩阵。 (注:这里对泰勒展开式只保留到一阶导,二阶导数以上的都舍去,噪声假设均为加性高斯噪声) 基于以上的公式,给出EKF的预测(Predict)和更新(Update)两个步骤: Propagation: Update: 其中的雅克比矩阵和分别为 , 雅可比矩阵的计算,在MATLAB中可以利用对自变量加上一个eps(极小数),然后用因变量的变化量去除以eps即可得到雅可比矩阵的每一个元素值。 读者可能好奇?为什么扩展卡尔曼滤波EKF的传播和更新的形式会和标准卡尔曼滤波KF的形式一致呢?以下做一个简单的推导。 3 推导先列出几个变量的表示、状态转移方程和观测方程: 真实值,预测值,估计值,观测值,观测值的预测,估计值与真实值之间的误差协方差矩阵,求期望的符号。 , , 引入反馈: (5) OK,可以开始推导了: 由公式(3)(4)得到以下两个等式,标为式(6)(7) 计算估计值与真实值之间的误差协方差矩阵,并把式子(4)(5)(7)代入,得到 其中表示真实值与与预测值之间的误差协方差矩阵。于是得到式(8) 因为的对角元即为真实值与估计值的误差的平方,矩阵的迹(用表示)即为总误差的平方和,即 利用以下矩阵迹的求导公式(其中和表示矩阵,表示列向量): 要让估计值更接近于真实值,就要使上面的迹尽可能的小,因此要取得合适的卡尔曼增益,使得迹得到最小,言外之意就是使得迹对的偏导为0,即 这样就能算出合适的卡尔曼增益了,即 代回式(8)得到 接下来就差真实值与预测值之间的协方差矩阵的求值公式了 将以下两个等式代入到上式, , 可以得到 有、与观测噪声是独立的,求期望等于零;;表示观测噪声的协方差矩阵,用表示。于是得到 其中的协方差矩阵的转置矩阵就是它本身。OK,大功告成,以上就完成了全部公式的推导了。 4 实际应用现在我们假设在海上有一艘正在行驶的船只,其相对于传感器的横纵坐标为为隐藏状态,无法直接获得,而传感器可以测量得到船只相对于传感器的距离和角度,传感器采样的时间间隔为,则: 状态向量,观测向量 状态转移方程和观测方程为: 那么雅克比矩阵为 这里给定距离传感器的噪声均值为,方差为;角度传感器的噪声均值为0,方差为(单位弧度); 采样时间点为个; 船只的初始状态为,四个状态量的噪声的方差分别为。仿真结果如下: 从仿真结果可以看出,EKF在这种情形下的滤波效果还是不错的,但是在实际应用中,对于船只运动的状态转移噪声的均值和协方差矩阵,以及传感器的观测噪声的均值和协方差矩阵,往往都是未知的,有很多情况都只有观测值而已,这样的情形下,就有必要利用观测值对噪声的统计量参数做出适当的估计(学习)。 5 参数估计(参数学习)利用EM算法和极大后验概率估计(MAP),对未知的噪声参数做出估计,再利用估计出的参数去递推卡尔曼滤波的解。本文对EM算法在卡尔曼滤波框架中的推导暂时先不给出,之后可能会补充,这里就先给出一种Adaptive-EKF算法的公式。 , , (1)E-Step Propagation: Update: (2)M-Step 利用以上的Adaptive-EKF算法对船只的运动做滤波跟踪,得到的效果如下图所示: 相比于没有做参数估计的EKF滤波,可以看出,Adaptive-EKF在估计误差上要优于EKF滤波,而且,它并不需要指定状态转移噪声和观测噪声的参数,将更有利于在实际中的应用。 6 总结EKF滤波通过泰勒展开公式,把非线性方程在局部线性化,使得高斯分布的变量在经过线性变换后仍然为高斯分布,这使得能继续把标准卡尔曼滤波KF的框架拿过来用,总体来说,EKF在函数的非线性不是很剧烈的情形下,能够具有很不错的滤波效果。但是EKF也有它的不足之处:其一,它必须求解非线性函数的Jacobi矩阵,对于模型复杂的系统,它比较复杂而且容易出错;其二,引入了线性化误差,对非线性强的系统,容易导致滤波结果下降。基于以上原因,为了提高滤波精度和效率,以满足特殊问题的需要,就必须寻找新的逼近方法,于是便有了粒子滤波PF和无迹卡尔曼滤波UKF,笔者将在接下来的博文中为读者解读。 7 参考文献[1] 林鸿. 基于EM算法的随机动态系统建模[J]. 福建师大学报(自然科学版), 2011, 27(6):33-37. [2] https://www.cnblogs.com/gaoxiang12/p/5560360.html. [3] https://max.book118.com/html/2017/0502/103920556.shtm. 原创性声明:本文属于作者原创性文章,小弟码字辛苦,转载还请注明出处。谢谢~ 如果有哪些地方表述的不够得体和清晰,有存在的任何问题,亦或者程序存在任何考虑不周和漏洞,欢迎评论和指正,谢谢各路大佬。 需要代码和有需要相关技术支持的可咨询QQ:297461921 |
今日新闻 |
推荐新闻 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |