前言

经典卡尔曼滤波的使用场景是线性系统，但现实应用时，大多数系统都是非线性的。扩展卡尔曼滤波（Extended Kalman Filter）是针对非线性系统的卡尔曼滤波方法。

非线性系统

经典卡尔曼滤波的状态和观测方程都是线性方程：
xn=Axn−1+Bun+wnzn=Hxn+vnx_n=Ax_{n-1}+Bu_n+w_n \\ z_n=Hx_n + v_n xn=Axn−1+Bun+wnzn=Hxn+vn

如果状态方程或者观测方程是非线性的，则有：
xn=f(xn−1,un)+wnzn=g(xn)+vnx_n=f(x_{n-1},u_n)+w_n \\ z_n=g(x_n)+v_n xn=f(xn−1,un)+wnzn=g(xn)+vn
由于控制输入并不本质，后续讨论不会涉及unu_nun。

状态/观测方程线性化

可以根据泰勒公式，将函数展开为n阶多项式拟合：
f(x)=f(x0)+f′(x0)(x−x0)+12f′′(x0)(x−x0)2+o((x−x0)2)f(x)=f(x_0)+f'(x_0)(x-x_0)+ \frac {1}{2}f''(x_0)(x-x_0)^2+o((x-x_0)^2) f(x)=f(x0)+f′(x0)(x−x0)+21f′′(x0)(x−x0)2+o((x−x0)2)

因此，可以通过泰勒公式将非线性的状态方程和观测方程进行线性化：
xn=f(x^n−1)+f′(x^n−1)(xn−1−x^n−1)+wnzn=g(x^n∣n−1)+g′(x^n∣n−1)(xn−x^n∣n−1)+vnx_n=f(\hat x_{n-1})+f'(\hat x_{n-1})(x_{n-1}- \hat x_{n-1})+w_n \\ z_n = g(\hat x_{n|n-1})+g'(\hat x_{n|n-1})(x_n - \hat x_{n|n-1}) +v_n xn=f(x^n−1)+f′(x^n−1)(xn−1−x^n−1)+wnzn=g(x^n∣n−1)+g′(x^n∣n−1)(xn−x^n∣n−1)+vn
以上线性化有几点值得注意：

EKF线性化使用了一阶近似，算是相对简单；也可以使用二阶或者更高阶的近似。
由于卡尔曼增益需要计算测量残差，因此观测方程是由状态更新点x^n∣n−1\hat x_{n|n-1}x^n∣n−1展开的。

将系统的状态方程和观测方程线性化后，就可以直接通过线性卡尔曼滤波的方法开始推导了，过程是完全一样的。

扩展卡尔曼滤波EKF

首先，把线性卡尔曼滤波的五个公式列出来：
KF:prediction:x^n∣n−1=Ax^n−1P^n∣n−1=AP^n−1AT+Qupdate:Kn=P^n∣n−1HT(HPnHT+R)−1x^n=x^n∣n−1+Kk(z^n−Hx^n∣n−1)Pn=(E−KkH)P^n∣n−1\begin{aligned} KF: \\ prediction : & \\ & \hat x_{n|n-1} = A\hat x_{n-1} &\quad \\ & \hat P_{n|n-1}=A\hat P_{n-1}A^T+Q &\quad \\ update: & \\ & K_n = \hat P_{n|n-1}H^T(HP_nH^T+R)^{-1} \\ & \hat x_n = \hat x_{n|n-1} + K_k(\hat z_n - H \hat x_{n|n-1}) \\ & P_n = (E-K_kH)\hat P_{n|n-1} \end{aligned} KF:prediction:update:x^n∣n−1=Ax^n−1P^n∣n−1=AP^n−1AT+QKn=P^n∣n−1HT(HPnHT+R)−1x^n=x^n∣n−1+Kk(z^n−Hx^n∣n−1)Pn=(E−KkH)P^n∣n−1

扩展卡尔曼滤波，可以直接通过非线性化，将f(x),g(x)f(x),g(x)f(x),g(x)这样的非线性函数，近似为局部区域内的线性函数：
f′(x^n−1)=Ax∼Ag′(x^n−1)=Hx∼Hf'(\hat x_{n-1})=A_x \sim A \\ g'(\hat x_{n-1})=H_x \sim H \\ f′(x^n−1)=Ax∼Ag′(x^n−1)=Hx∼H

注意，测量误差可以直接使用非线性观测方程获得：
z^n∣n−1=g(x^n∣n−1)z^n−z^n∣n−1=z^n−g(x^n∣n−1)\hat z_{n|n-1}=g(\hat x_{n|n-1})\\ \hat z_n - \hat z_{n|n-1}=\hat z_n-g(\hat x_{n|n-1}) z^n∣n−1=g(x^n∣n−1)z^n−z^n∣n−1=z^n−g(x^n∣n−1)

然后，将线性卡尔曼滤波五个公式的参数进行替换，就获得了扩展卡尔曼滤波：
EKF:prediction:x^n∣n−1=f(x^n−1)P^n∣n−1=AxP^n−1AxT+Qupdate:Kn=P^n∣n−1HxT(HxPnHxT+R)−1x^n=x^n∣n−1+Kk(z^n−g(x^n∣n−1))Pn=(E−KkHx)P^n∣n−1where:f′(x^n−1)=Axg′(x^n−1)=Hx\begin{aligned} EKF: \\ prediction : & \\ & \hat x_{n|n-1} = f(\hat x_{n-1}) &\quad \\ & \hat P_{n|n-1}=A_x\hat P_{n-1}A_x^T+Q &\quad \\ update: & \\ & K_n = \hat P_{n|n-1}H_x^T(H_xP_nH_x^T+R)^{-1} \\ & \hat x_n = \hat x_{n|n-1} + K_k(\hat z_n - g(\hat x_{n|n-1})) \\ & P_n = (E-K_kH_x)\hat P_{n|n-1} \\ where: \\ & f'(\hat x_{n-1})=A_x \\ & g'(\hat x_{n-1})=H_x \\ \end{aligned} EKF:prediction:update:where:x^n∣n−1=f(x^n−1)P^n∣n−1=AxP^n−1AxT+QKn=P^n∣n−1HxT(HxPnHxT+R)−1x^n=x^n∣n−1+Kk(z^n−g(x^n∣n−1))Pn=(E−KkHx)P^n∣n−1f′(x^n−1)=Axg′(x^n−1)=Hx

KF和EKF的预测和更新是一模一样的。可以根据卡尔曼增益的推导推出这个结论。

后记

总结一下，扩展卡尔曼滤波EKF，实际上就是将非线性系统的状态和观测方程进行线性化后的线性卡尔曼滤波。

控制算法学习四、扩展卡尔曼滤波EKF相关推荐

控制算法学习一、卡尔曼滤波（1）以小车为例
控制算法学习一.卡尔曼滤波(1)以小车为例前言线性系统状态方程和观测方程举例卡尔曼滤波状态预测状态更新卡尔曼滤波流程图前言卡尔曼滤波(Kalman Filter, KF)是最经典 ...
基于扩展卡尔曼滤波EKF和模型预测控制MPC，自动泊车场景建模开发
基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB 基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB(工程项目线上支持 ...
基于扩展卡尔曼滤波(EKF)的机器人状态估计
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 EKF的目的是使卡尔曼滤波器能够应用于机器人等非线性运动系统,EK ...
扩展卡尔曼滤波EKF与多传感器融合
Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本.在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准.本文将简要介绍EKF,并介绍其在无人驾 ...
卫星轨道的估计问题(Matlab)(二):扩展卡尔曼滤波(EKF)对新问题的尝试
前言在前面的问题中我们已经考虑到了用微分方程来描述卫星运动轨迹的方法: r¨=rθ˙2−GMr−2θ¨=−2r−1r˙θ˙\ddot r = r\dot \theta^2-GMr^{-2}\\\dd ...
卡尔曼滤波KF与扩展卡尔曼滤波EKF算法
最近学习了一些预测算法卡尔曼滤波具体推导可以查看B站的教程:贝叶斯滤波与卡尔曼滤波第一讲很有必要的绪论_哔哩哔哩_bilibili 建议从第一讲开始看看上述教程花不了多少时间,可能看一个电影的 ...
基于扩展卡尔曼滤波(EKF)和低成本传感器的AHRS实现
文章目录传感器观测向量观测矩阵状态向量系统矩阵测试结果传感器使用华为honor7手机采集加速度计,陀螺仪和磁力计数据. 加速计数据在使用前经过低通滤波器.滤波结果如下图: 观测向量如 ...
卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）相应推导
从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的连接上做出了卓越的贡献.为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波.扩展卡尔曼滤波.无迹卡尔曼滤波.粒子滤波等等方法,这些方法在 ...
概率机器人总结——(扩展)卡尔曼滤波先实践再推导
概率机器人总结--卡尔曼滤波先实践再推导概率机器人总结--(扩展)卡尔曼滤波先实践再推导 (1)卡尔曼.扩展卡尔曼.粒子滤波到底什么关系? (2)实践--扩展卡尔曼滤波 (3)推导--卡尔曼滤波 ( ...

控制算法学习四、扩展卡尔曼滤波EKF

控制算法学习四、扩展卡尔曼滤波EKF

前言

非线性系统

状态/观测方程线性化

扩展卡尔曼滤波EKF

后记

控制算法学习四、扩展卡尔曼滤波EKF相关推荐

最新文章

热门文章

控制算法学习 四、扩展卡尔曼滤波EKF

控制算法学习 四、扩展卡尔曼滤波EKF

前言

非线性系统

状态/观测方程线性化

扩展卡尔曼滤波EKF

后记

控制算法学习 四、扩展卡尔曼滤波EKF相关推荐

最新文章

热门文章

控制算法学习四、扩展卡尔曼滤波EKF

控制算法学习四、扩展卡尔曼滤波EKF

控制算法学习四、扩展卡尔曼滤波EKF相关推荐