扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）

文章目录

扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）理论讲解KF和EKF模型对比雅可比矩阵计算矩阵计算实例应用实例线性模型CV模型：CA模型非线性模型CTRV模型：CTRV示例python代码C++代码matlab示例抛物线demomatlab代码C++示例飞机高度demo1C++代码飞机高度demo2C++代码EKF的不足参考文献

我们上篇提到的卡尔曼滤波（参见我的另一篇文章：卡尔曼滤波理论讲解与应用（matlab和python））是用于线性系统，预测（运动）模型和观测模型是在假设高斯和线性情况下进行的。简单的卡尔曼滤波必须应用在符合高斯分布的系统中，但是现实中并不是所有的系统都符合这样 。另外高斯分布在非线性系统中的传递结果将不再是高斯分布。那如何解决这个问题呢？扩展卡尔曼滤波就是干这个事的。

理论讲解

扩展卡尔曼滤波（Extended Kalman Filter，EKF）通过局部线性来解决非线性的问题。将非线性的预测方程和观测方程进行求导，以切线代替的方式来线性化。其实就是在均值处进行一阶泰勒展开。

数学中，泰勒公式是一个用函数在某点的信息描述其附近取值的公式（ 一句话描述：就是用多项式函数去逼近光滑函数 ）。如果函数足够平滑的话，在已知函数在某一点的各阶导数值的情况之下，泰勒公式可以用这些导数值做系数构建一个多项式来近似函数在这一点的邻域中的值。泰勒公式还给出了这个多项式和实际的函数值之间的偏差。

f(x)Taylor=∑n=0∞f(n)(a)n!×(x−a)n=f(a)+f′(a)1!(x−a)+f(2)(a)2!(x−a)2+⋯+f(n)(a)n!(x−a)n+Rn(x)f(x)_{Taylor} = \sum_{n=0}^{\infty} \frac{f^{(n)}(a)}{n!} \times (x - a)^n\\ =f(a) + \frac{f'(a)}{1!}(x-a) + \frac{f^{(2)}(a)}{2!}(x-a)^2+ \cdots + \frac{f^{(n)}(a)}{n!}(x-a)^n + R_n(x) f(x)Taylor​=n=0∑∞​n!f(n)(a)​×(x−a)n=f(a)+1!f′(a)​(x−a)+2!f(2)(a)​(x−a)2+⋯+n!f(n)(a)​(x−a)n+Rn​(x)

f(n)(a)f^{(n)}(a)f(n)(a) 表示 f(x)f(x)f(x) 在第 nnn阶导数的表达式，带入一个值aaa计算后得到的结果（注意，它是个值）

1n!\frac{1}{n!}n!1​是一个系数（一个值），每一项都不同，第一项 11\frac{1}{1}11​，第二项12\frac{1}{2}21​ …… 依此类推

(x−a)n(x−a)^n(x−a)n是一个以xxx为自变量的表达式。Rn(x)Rn(x)Rn(x)是泰勒公式的余项，是 (x−a)n(x−a)^n(x−a)n 的高阶无穷小

KF和EKF模型对比

首先，让卡尔曼先和扩展卡尔曼滤波做一个对比。在对比过程中可以看出，扩展卡尔曼是一个简单的非线性近似滤波算法，指运动或观测方程不是线性的情况,在预测模型部分，扩展卡尔曼的预测模型和量测模型已经是非线性了。为了简化计算，EKF通过一阶泰勒分解线性化运动、观测方程。KF与EKF具有相同的算法结构，都是以高斯形式描述后验概率密度的，通过计算贝叶斯递推公式得到的。最大的不同之处在于，计算方差时，EKF的状态转移矩阵（上一时刻的状态信息k−1∣k−1k-1|k-1k−1∣k−1）和观测矩阵（一步预测k∣k−1k|k-1k∣k−1）都是状态信息的雅克比矩阵（ 偏导数组成的矩阵）。 在预测公式部分，扩展卡尔曼滤波的FkF_kFk​为fff的雅可比矩阵，在更新公式部分，扩展卡尔曼滤波的HkH_kHk​为hhh的雅可比矩阵。

预测模型：

KalmanFilterExtendedKalmanFilterxk=Fxk−1+Buk+wkxk=f(xk−1,uk)+wkzk⃗=Hxk+vkzk⃗=h(xk)+vk\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter\\ x_k=Fx_{k-1}+Bu_k+w_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad x_k=f(x_{k-1},u_k)+w_k\\ \vec{z_k}=Hx_k+v_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \vec{z_k}=h(x_k)+v_k KalmanFilterExtendedKalmanFilterxk​=Fxk−1​+Buk​+wk​xk​=f(xk−1​,uk​)+wk​zk​​=Hxk​+vk​zk​​=h(xk​)+vk​

预测公式：

KalmanFilterExtendedKalmanFilterx^k=Fkx^k−1+Bku⃗kx^k=f(xk−1,uk)Pk=FkPk−1FkT+QkPk=FkPk−1FkT+Qk\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter\\ \hat x_k=F_k \hat x_{k-1} + B_k \vec u_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \hat x_k=f(x_{k-1},u_k)\\ P_k=F_kP_{k-1}F^T_k+Q_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad P_k=F_kP_{k-1}F^T_k+Q_k KalmanFilterExtendedKalmanFilterx^k​=Fk​x^k−1​+Bk​uk​x^k​=f(xk−1​,uk​)Pk​=Fk​Pk−1​FkT​+Qk​Pk​=Fk​Pk−1​FkT​+Qk​

更新公式：

KalmanFilterExtendedKalmanFilterK′=PkHkT(HkPkHkT+Rk)−1K′=PkHkT(HkPkHkT+Rk)−1x^k′=x^k+K′(zk⃗−Hkx^k)x^k′=x^k+K′(zk⃗−h(x^k))Pk′=Pk−K′HkPkPk′=Pk−K′HkPk\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter \\ K' = P_kH^T_k(H_kP_kH^T_k+R_k)^{-1} \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad K' = P_kH^T_k(H_kP_kH^T_k+R_k)^{-1} \\ \hat x'_k = \hat x_k+K'(\vec {z_k}-H_k\hat x_k) \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \hat x'_k = \hat x_k+K'(\vec {z_k}-h(\hat x_k)) \\ P'_k=P_k-K'H_kP_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad P'_k=P_k-K'H_kP_k KalmanFilterExtendedKalmanFilterK′=Pk​HkT​(Hk​Pk​HkT​+Rk​)−1K′=Pk​HkT​(Hk​Pk​HkT​+Rk​)−1x^k′​=x^k​+K′(zk​​−Hk​x^k​)x^k′​=x^k​+K′(zk​​−h(x^k​))Pk′​=Pk​−K′Hk​Pk​Pk′​=Pk​−K′Hk​Pk​

其中

xk^\hat{x_k}xk​^​为kkk时刻的状态向量；

FkF_kFk​为状态转移矩阵，表示将k−1k-1k−1时刻的状态向量转移至t时刻的状态向量；

BkB_kBk​是输入控制矩阵，代表着将控制向量 uk^\hat{u_k}uk​^​映射到状态向量上，统一控制向量 uk^\hat{u_k}uk​^​和状态向量之间 xk^\hat{x_k}xk​^​的关系；

uk^\hat{u_k}uk​^​代表着控制向量，如加速度，角加速度等；

wkw_kwk​为过程演化噪声；

vkv_kvk​为量测噪声 ；

PkP_kPk​为状态向量的协方差矩阵，代表着状态向量每个元素之间的关系；

QkQ_kQk​表示预测状态的高斯噪声的协方差阵，它用来衡量模型的准确度，模型越准确其值越小；

zk⃗\vec {z_k}zk​​为传感器测量值的状态向量，也就是传感器的测量结果；

HkH_kHk​ 为转换矩阵，他将状态向量 xk^\hat{x_k}xk​^​映射到测量值所在的向量空间zk⃗\vec {z_k}zk​​；

RkR_kRk​为测量值的高斯噪声的协方差阵，代表着传感器测量的误差。

图片示例：

JA,JHJ_A,J_HJA​,JH​均为雅可比矩阵。

雅可比矩阵计算

雅可比矩阵（Jacobian matrix ）是该函数f(x)f(x)f(x) 的所有行数（mmm个）对状态向量x⃗k\vec x_kxk​的所有分量（nnn 个）的一阶偏导数组成的矩阵。雅可比矩阵FkF_kFk​的计算如下，其中输入噪声为0。

xk=f(xk−1,uk，wk)≈f(x^k−1,uk,0)+∂f∂xk−1∣x^k−1,uk,0⏟Fk−1(xk−1−x^k−1)+∂f∂wk−1∣x^k−1,uk,0⏟Lk−1wk令k=k−1Fk=def∂f(x)∂x=def[∂f∂x1...∂f∂xn]=def[∂f1∂x1...∂f1∂xn⋮⋮⋮∂fm∂x1...∂fm∂xn]=defJA.x_k=f(x_{k-1},u_k，w_k) \approx f(\hat x_{k-1},u_k,0)+ \underbrace{\left.\frac{\partial {f}}{\partial {x}_{k-1}}\right|_{\hat{{x}}_{k-1}, {u}_{k},0} }_{\mathbf{F}_{k-1}} \left({x}_{k-1}-\hat{{x}}_{k-1}\right) + \underbrace{\left.\frac{\partial {f}}{\partial {w}_{k-1}}\right|_{\hat{{x}}_{k-1}, {u}_{k},0} }_{\mathbf{L}_{k-1}} w_k\\令k=k-1\\F_k\stackrel{def}{=}\frac{\partial f(x)}{\partial x}\stackrel{def}{=} \left[ \begin{array}{ccc} \frac{\partial f}{\partial x_1} & ... & \frac{\partial f}{\partial x_n} \\ \end{array} \right] \stackrel{def}{=} \left[ \begin{array}{ccc} \frac{\partial f_1}{\partial x_1} & ... & \frac{\partial f_1}{\partial x_n} \\ \vdots & \vdots & \vdots \\\frac{\partial f_m}{\partial x_1} & ... & \frac{\partial f_m}{\partial x_n} \end{array} \right] \stackrel{def}{=} J_A.\\ xk​=f(xk−1​,uk​，wk​)≈f(x^k−1​,uk​,0)+Fk−1​∂xk−1​∂f​∣∣∣∣​x^k−1​,uk​,0​​​(xk−1​−x^k−1​)+Lk−1​∂wk−1​∂f​∣∣∣∣​x^k−1​,uk​,0​​​wk​令k=k−1Fk​=def∂x∂f(x)​=def[∂x1​∂f​​...​∂xn​∂f​​]=def⎣⎢⎡​∂x1​∂f1​​⋮∂x1​∂fm​​​...⋮...​∂xn​∂f1​​⋮∂xn​∂fm​​​⎦⎥⎤​=defJA​.

函数f(xk−1,uk,wk)f(x_{k-1},u_k,w_k)f(xk−1​,uk​,wk​)有f1,...,fmf_1,...,f_mf1​,...,fm​ 行个分量，于是有mmm行。状态向量x⃗k\vec x_kxk​有x1,...,xnx_1,...,x_nx1​,...,xn​个分量，于是有nnn列。

雅可比矩阵HkH_kHk​的计算如下，其中输入噪声为0。

yk=h(xk,vk)≈h(x^k,0)+∂h∂xk∣x^k,0⏟Hk(xk−x^k)+∂h∂vk∣x^k,0⏟MkvkHk=def∂h(x)∂x=def[∂h∂x1...∂h∂xn]=def[∂h1∂x1...∂h1∂xn⋮⋮⋮∂hm∂x1...∂hm∂xn]=defJH.y_k=h(x_{k},v_k) \approx h(\hat x_{k},0)+ \underbrace{\left.\frac{\partial {h}}{\partial {x}_{k}}\right|_{\hat{{x}}_{k}, 0} }_{\mathbf{H}_{k}} \left({x}_{k}-\hat{{x}}_{k}\right) + \underbrace{\left.\frac{\partial {h}}{\partial {v}_{k}}\right|_{\hat{{x}}_{k}, 0} }_{\mathbf{M}_{k}} v_k \\H_k\stackrel{def}{=}\frac{\partial h(x)}{\partial x}\stackrel{def}{=} \left[ \begin{array}{ccc} \frac{\partial h}{\partial x_1} & ... & \frac{\partial h}{\partial x_n} \\ \end{array} \right] \stackrel{def}{=} \left[ \begin{array}{ccc} \frac{\partial h_1}{\partial x_1} & ... & \frac{\partial h_1}{\partial x_n} \\ \vdots & \vdots & \vdots \\ \frac{\partial h_m}{\partial x_1} & ... & \frac{\partial h_m}{\partial x_n} \end{array} \right] \stackrel{def}{=} J_H. yk​=h(xk​,vk​)≈h(x^k​,0)+Hk​∂xk​∂h​∣∣∣∣​x^k​,0​​​(xk​−x^k​)+Mk​∂vk​∂h​∣∣∣∣​x^k​,0​​​vk​Hk​=def∂x∂h(x)​=def[∂x1​∂h​​...​∂xn​∂h​​]=def⎣⎢⎡​∂x1​∂h1​​⋮∂x1​∂hm​​​...⋮...​∂xn​∂h1​​⋮∂xn​∂hm​​​⎦⎥⎤​=defJH​.

函数h(xk,vk)h(x_{k},v_k)h(xk​,vk​)有h1,...,hmh_1,...,h_mh1​,...,hm​ 行个分量，于是有mmm行。状态向量x⃗k\vec x_kxk​ 有x1,...,xnx_1,...,x_nx1​,...,xn​个分量，于是有nnn列。

矩阵计算实例

举例说明：f(x)f(x)f(x)有f1,f2f_1,f_2f1​,f2​两个函数，状态向量x⃗k\vec x_kxk​有x1,x2x_1,x_2x1​,x2​两个分量，所以有

f(x)=[f1f2]=[x1+x2x12]∂f∂x=[∂f1∂x1∂f1∂x2∂f2∂x1∂f2∂x2]=[112x10]{f}({x})=\left[\begin{array}{l} f_{1} \\ f_{2} \end{array}\right]=\left[\begin{array}{c} x_{1}+x_{2} \\ x_{1}^{2} \end{array}\right] \quad \quad \frac{\partial {f}}{\partial {x}}=\left[\begin{array}{cc} \frac{\partial f_{1}}{\partial x_{1}} & \frac{\partial f_{1}}{\partial x_{2}} \\ \frac{\partial f_{2}}{\partial x_{1}} & \frac{\partial f_{2}}{\partial x_{2}} \end{array}\right]=\left[\begin{array}{cc} 1 & 1 \\ 2 x_{1} & 0 \end{array}\right] f(x)=[f1​f2​​]=[x1​+x2​x12​​]∂x∂f​=[∂x1​∂f1​​∂x1​∂f2​​​∂x2​∂f1​​∂x2​∂f2​​​]=[12x1​​10​]

又如：

应用实例

线性模型

常用的线性模型有

恒定速度模型（Constant Velocity, CV）恒定加速度模型（Constant Acceleration, CA）

这些线性运动模型假定目标是直线运动的，并不考虑物体的转弯。

CV模型：

CV模型的状态空间可以表示为：

x→(t)=(xyvxvy)T\overrightarrow{x}(t) = (x \quad y \quad v_x \quad v_y)^T x(t)=(xyvx​vy​)T

那么转移函数为：

x→(t+Δt)=(x(t)+Δtvxy(t)+Δtvyvxvy)\overrightarrow{x}(t+\Delta t) = \left( \begin{array}{c} x(t) + \Delta t v_x \\ y(t) + \Delta t v_y \\ v_x \\ v_y \end{array} \right) x(t+Δt)=⎝⎜⎜⎛​x(t)+Δtvx​y(t)+Δtvy​vx​vy​​⎠⎟⎟⎞​

或者，CV模型的完整的状态空间还可以表示为：

x→(t)=(xyvxvyaxay)T\overrightarrow{x}(t) = (x \quad y \quad v_x \quad v_y \quad a_x \quad a_y)^T x(t)=(xyvx​vy​ax​ay​)T

矩阵表示为：

x1→=(x1y1vx1vy1ax1ay1)=F⋅x0→=(10t000010t00001000000100000000000000)⋅(x0y0vx0vy0ax0ay0)=(x0+vx0ty0+vy0tvx0vy000)\overrightarrow{x1} =\left( \begin{array}{c} x_1\\ y_1\\ v_{x1}\\ v_{y1}\\ a_{x1}\\ a_{y1} \end{array} \right)= F \cdot \overrightarrow{x0}= \left( \begin{array}{c} 1 & 0 & t & 0 & 0 &0\\ 0 & 1 & 0 & t & 0 &0\\ 0 & 0 & 1 & 0 & 0 &0\\ 0 & 0 & 0 & 1 & 0 &0\\ 0 & 0 & 0 & 0 & 0 &0 \\ 0 & 0 & 0 & 0 & 0 &0 \end{array} \right) \cdot \left( \begin{array}{c} x_0\\ y_0\\ v_{x0}\\ v_{y0}\\ a_{x0}\\ a_{y0} \end{array} \right)= \left( \begin{array}{c} x_0+v_{x0}t\\ y_0+v_{y0}t\\ v_{x0}\\ v_{y0}\\ 0\\ 0 \end{array} \right) x1=⎝⎜⎜⎜⎜⎜⎜⎛​x1​y1​vx1​vy1​ax1​ay1​​⎠⎟⎟⎟⎟⎟⎟⎞​=F⋅x0=⎝⎜⎜⎜⎜⎜⎜⎛​100000​010000​t01000​0t0100​000000​000000​⎠⎟⎟⎟⎟⎟⎟⎞​⋅⎝⎜⎜⎜⎜⎜⎜⎛​x0​y0​vx0​vy0​ax0​ay0​​⎠⎟⎟⎟⎟⎟⎟⎞​=⎝⎜⎜⎜⎜⎜⎜⎛​x0​+vx0​ty0​+vy0​tvx0​vy0​00​⎠⎟⎟⎟⎟⎟⎟⎞​

CA模型

CA模型的状态空间可以表示为：

x→(t)=(xyvxvyaxay)T\overrightarrow{x}(t) = (x \quad y \quad v_x \quad v_y \quad a_x \quad a_y)^T x(t)=(xyvx​vy​ax​ay​)T

矩阵表示为：

x1→=(x1y1vx1vy1ax1ay1)=F⋅x0→=(10t012t20010t012t20010t000010t000010000001)⋅(x0y0vx0vy0ax0ay0)=(x0+vx0t+12ax0t2y0+vy0t+12ay0t2vx0+ax0tvy0+ay0tax0ay0)\overrightarrow{x1} =\left( \begin{array}{c} x_1\\ y_1\\ v_{x1}\\ v_{y1}\\ a_{x1}\\ a_{y1} \end{array} \right)= F \cdot \overrightarrow{x0}= \left( \begin{array}{c} 1 & 0 & t & 0 & \frac{1}{2}t^2 &0\\ 0 & 1 & 0 & t & 0 &\frac{1}{2}t^2\\ 0 & 0 & 1 & 0 & t &0\\ 0 & 0 & 0 & 1 & 0 &t\\ 0 & 0 & 0 & 0 & 1 &0 \\ 0 & 0 & 0 & 0 & 0 &1 \end{array} \right) \cdot \left( \begin{array}{c} x_0\\ y_0\\ v_{x0}\\ v_{y0}\\ a_{x0}\\ a_{y0} \end{array} \right)= \left( \begin{array}{c} x_0+v_{x0}t+\frac{1}{2}a_{x0}t^2\\ y_0+v_{y0}t+\frac{1}{2}a_{y0}t^2\\ v_{x0}+a_{x0}t\\ v_{y0}+a_{y0}t\\ a_{x0}\\ a_{y0} \end{array} \right) x1=⎝⎜⎜⎜⎜⎜⎜⎛​x1​y1​vx1​vy1​ax1​ay1​​⎠⎟⎟⎟⎟⎟⎟⎞​=F⋅x0=⎝⎜⎜⎜⎜⎜⎜⎛​100000​010000​t01000​0t0100​21​t20t010​021​t20t01​⎠⎟⎟⎟⎟⎟⎟⎞​⋅⎝⎜⎜⎜⎜⎜⎜⎛​x0​y0​vx0​vy0​ax0​ay0​​⎠⎟⎟⎟⎟⎟⎟⎞​=⎝⎜⎜⎜⎜⎜⎜⎛​x0​+vx0​t+21​ax0​t2y0​+vy0​t+21​ay0​t2vx0​+ax0​tvy0​+ay0​tax0​ay0​​⎠⎟⎟⎟⎟⎟⎟⎞​

CA和CV的交互式多模型可以参考我的这篇博客：交互式多模型 IMM的原理及代码实现（matlab）

非线性模型

非线性模型包括：

恒定转率和速度模型（Constant Turn Rate and Velocity，CTRV）恒定转率和加速度模型（Constant Turn Rate and Acceleration，CTRA）

CTRV目前多用于机载追踪系统（飞机），这些二次运动模型大多假定速度 vvv和 偏航角速度（yaw rate） ωωω没有关系，因此，在这类运动模型中，由于偏航角速度测量的扰动（不稳定），即使车辆没有移动，我们的运动模型下的角速度也会发生细微的变化。为了解决这个问题，速度 vvv 和 偏航角速度 ωωω 的关联可以通过设定转向角 ΦΦΦ恒定来建立，这样就引出了 恒定转向角和速度模型（Constant Steering Angle and Velocity，CSAV）, 另外，速度可以别假定为线性变化的，进而引出了常曲率和加速度模型（Constant Curvature and Acceleration，CCA）。

CTRV模型：

CA、CV模型并不能预测预测转弯，为了解决这个问题我们使用CTRV模型:

在CTRV中，使用位置px,pyp_x,p_ypx​,py​,速度vvv，角度ψ\psiψ，及角速度ψ˙\dot{\psi}ψ˙​来作为目标的状态向量：

x→(t)=(pxpyvψψ˙)T\overrightarrow{x}(t) = (p_x \quad p_y \quad v \quad \psi \quad \dot{\psi})^T x(t)=(px​py​vψψ˙​)T

其中，ψ\psiψ为偏航角，是追踪的目标车辆在当前车辆坐标系下与xxx轴的夹角，逆时针方向为正，取值范围是[0,2π)[0, 2π)[0,2π), ψ˙\dot{\psi}ψ˙​是偏航角速度。

其推导过程为：

CTRV的状态向量也可以表示为：

x→(t)=(xyvθω)T\overrightarrow{x}(t) = (x \quad y \quad v \quad \theta \quad \omega)^T x(t)=(xyvθω)T

其中，θθθ为偏航角，ωωω是偏航角速度。

CTRV的状态转移函数为：

x→(t+Δt)=(vωsin⁡(ωΔt+θ)−vωsin⁡(θ)+x(t)−vωcos⁡(ωΔt+θ)+vωcos⁡(θ)+y(t)vωΔt+θω)\overrightarrow{x}(t+\Delta t) = \left( \begin{array}{c} \frac{v}{\omega} \sin(\omega \Delta t + \theta) - \frac{v}{\omega}\sin(\theta) + x(t) \\ -\frac{v}{\omega} \cos(\omega \Delta t + \theta) + \frac{v}{\omega}\cos(\theta) + y(t) \\ v \\ \omega \Delta t + \theta \\ \omega \end{array} \right) x(t+Δt)=⎝⎜⎜⎜⎜⎛​ωv​sin(ωΔt+θ)−ωv​sin(θ)+x(t)−ωv​cos(ωΔt+θ)+ωv​cos(θ)+y(t)vωΔt+θω​⎠⎟⎟⎟⎟⎞​

使用CTRV还存在一个问题，那就是 ω=0ω=0ω=0 的情况，此时我们的状态转移函数公式中的(x,y)(x,y)(x,y)将变成无穷大。为了解决这个问题，我们考察一下ω=0ω=0ω=0 的情况，此时我们追踪的车辆实际上是直线行驶的，所以我们的 (x,y)(x,y)(x,y)的计算公式就变成了：

x(t+Δt)=vcos⁡(θ)Δt+x(t)y(t+Δt)=vsin⁡(θ)Δt+y(t)x(t+\Delta t) = v \cos(\theta)\Delta t + x(t)\\ y(t+\Delta t) = v \sin(\theta)\Delta t + y(t) x(t+Δt)=vcos(θ)Δt+x(t)y(t+Δt)=vsin(θ)Δt+y(t)

那么现在问题来了，我们知道，卡尔曼滤波仅仅用于处理线性问题，那么很显然我们现在的处理模型是非线性的，这个时候我们就不能简单使用卡尔曼滤波进行预测和更新了，此时预测的第一步变成了如下非线性函数：

xk+1=f(xk,uk)x_{k+1} = f(x_k, u_k) xk+1​=f(xk​,uk​)

其中，f(x)f(x)f(x)表示CTRV运动模型的状态转移函数，uku_kuk​ 表示处理噪声。为了解决非线性系统下的问题，我们引入扩展卡尔曼滤波（Extended Kalman Filter，EKF）

CTRV示例

应用场景：

假设我们有激光雷达和雷达两个传感器，它们分别以一定的频率来测量如下数据：

激光雷达：测量目标车辆的坐标 (x,y)(x,y)(x,y) 。这里的x,yx,yx,y是相对于我们的车辆的坐标系的，即我们的车辆为坐标系的原点，我们的车头为xxx轴，车的左侧为yyy轴。雷达：测量目标车辆在我们车辆坐标系下与本车的距离ρρρ，目标车辆与x轴的夹角 ψψψ，以及目标车辆与我们自己的相对距离变化率 ρ˙\dot ρρ˙​（本质上就是目标车辆的实际速度在我们和目标车辆的连线上的分量）

前面的卡尔曼滤波器中，我们使用一个测量矩阵 HHH 将预测的结果映射到测量空间，那是因为这个映射本身就是线性的，现在，我们使用毫米波雷达和激光雷达来测量目标车辆(我们把这个过程称为传感器融合)，这个时候会有两种情况，即：

激光雷达的测量模型仍然是线性的，其测量矩阵为：

HL=[1000001000]H_L = \left[ \begin{array}{c} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{array} \right] HL​=[10​01​00​00​00​]

将预测映射到激光雷达测量空间：

HLx→=(x,y)TH_L\overrightarrow{x} = (x, y)^T HL​x=(x,y)T

雷达的预测映射到测量空间是非线性的，其表达式为：

(ρψρ˙)=(x2+y2atan2⁡(y,x)vx+vyx2+y2)\left(\begin{matrix} \rho \\ \psi \\ \dot\rho \end{matrix}\right) = \left(\begin{matrix}\sqrt{x^{2} + y^{2}}\\\operatorname{atan_{2}}{\left (y,x \right )}\\\frac{v x + v y}{\sqrt{x^{2} + y^{2}}}\end{matrix}\right) ⎝⎛​ρψρ˙​​⎠⎞​=⎝⎜⎛​x2+y2​atan2​(y,x)x2+y2​vx+vy​​⎠⎟⎞​

此时我们使用 h(x)h(x)h(x)来表示这样一个非线性映射 ，下面就是对 h(x)h(x)h(x)求雅各比行列式，具体的算法见代码。

滤波效果：

具体的推导可以参考这篇博客：高级运动模型和扩展卡尔曼滤波

python代码

这里附上完整的python代码：python_EKF_CTRV 代码

C++代码

这里附上完整的C++代码：C++_EKF_CTRV 代码

matlab示例

下面举一些小的例子，便于我们理解应用

抛物线demo

matlab代码

close all;clear all;%% 真实轨迹模拟kx = .01; ky = .05; % 阻尼系数g = 9.8;% 重力t = 15; % 仿真时间Ts = 0.1; % 采样周期 len = fix(t/Ts); % 仿真步数dax = 3; day = 3; % 系统噪声X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值for k=2:lenx = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4); x = x + vx*Ts;vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;y = y + vy*Ts;vy = vy + (ky*vy^2-g+day*randn(1))*Ts;X(k,:) = [x, vx, y, vy];end%% 构造量测量dr = 8; dafa = 0.1; % 量测噪声for k=1:lenr = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);a = atan(X(k,1)/X(k,3))*57.3 + dafa*randn(1,1);Z(k,:) = [r, a];end%% ekf 滤波Qk = diag([0; dax/10; 0; day/10])^2;Rk = diag([dr; dafa])^2;Pk = 10*eye(4);Pkk_1 = 10*eye(4);x_hat = [0,40,400,0]';X_est = zeros(len,4);x_forecast = zeros(4,1);z = zeros(4,1);for k=1:len% 1 状态预测 x1 = x_hat(1) + x_hat(2)*Ts;vx1 = x_hat(2) + (-kx*x_hat(2)^2)*Ts;y1 = x_hat(3) + x_hat(4)*Ts;vy1 = x_hat(4) + (ky*x_hat(4)^2-g)*Ts;x_forecast = [x1; vx1; y1; vy1]; %预测值% 2 观测预测r = sqrt(x1*x1+y1*y1);alpha = atan(x1/y1)*57.3;y_yuce = [r,alpha]';% 状态矩阵vx = x_forecast(2); vy = x_forecast(4);F = zeros(4,4);F(1,1) = 1; F(1,2) = Ts;F(2,2) = 1-2*kx*vx*Ts;F(3,3) = 1; F(3,4) = Ts;F(4,4) = 1+2*ky*vy*Ts;Pkk_1 = F*Pk*F'+Qk;% 观测矩阵x = x_forecast(1); y = x_forecast(3);H = zeros(2,4);r = sqrt(x^2+y^2); xy2 = 1+(x/y)^2;H(1,1) = x/r; H(1,3) = y/r;H(2,1) = (1/y)/xy2; H(2,3) = (-x/y^2)/xy2;Kk = Pkk_1*H'*(H*Pkk_1*H'+Rk)^-1; %计算增益x_hat = x_forecast+Kk*(Z(k,:)'-y_yuce);%校正Pk = (eye(4)-Kk*H)*Pkk_1;X_est(k,:) = x_hat;end%% figure, hold on, grid on;plot(X(:,1),X(:,3),'-b');plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180));plot(X_est(:,1),X_est(:,3), 'r');xlabel('X'); ylabel('Y'); title('EKF simulation');legend('real', 'measurement', 'ekf estimated');axis([-5,230,290,530]);

上例代码的效果图示：

可以看出一开始收敛的不快，下面一个代码完美解决了该问题

这里附上一个完整的matlab代码，效果超好。

C++示例

飞机高度demo1

假设一架飞机以恒定水平速度飞行（高度不变），地面上有一个雷达可以发射电磁波测量飞机到雷达的距离rrr。

从图中可以看出，有如下关系：

θ=arctan(yx)r2=x2+y2\theta=arctan(\frac{y}{x})\\r^2=x^2+y^2 θ=arctan(xy​)r2=x2+y2

我们想知道某一时刻飞机的水平位置和垂直高度，以水平位置、水平速度、垂直高度作为状态变量：

x=(distancevelocityaltitude)=(xx˙y)\mathbf{\textbf{x}}=\left(\begin{array}{c}\text{distance} \\\text{velocity} \\\text{altitude}\end{array}\right)=\left(\begin{array}{c}x \\\dot{x} \\y\end{array}\right) x=⎝⎛​distancevelocityaltitude​⎠⎞​=⎝⎛​xx˙y​⎠⎞​

则观测值与状态变量之间的关系为：h(x^)=x2+y2h\left(\hat{x}\right)=\sqrt{x^2+y^2}h(x^)=x2+y2​，可以看出这是一个非线性的表达式。对于这个问题来说，观测方程的雅克比矩阵为：JH=[∂h∂x∂h∂x˙∂h∂y]J_H=\left[\frac{\partial h}{\partial x}\quad\frac{\partial h}{\partial \dot{x}}\quad\frac{\partial h}{\partial y}\text{ }\right]JH​=[∂x∂h​∂x˙∂h​∂y∂h​]，即

JH=[xx2+y20yx2+y2]J_H=\left[\frac{x}{\sqrt{x^2+y^2}} \quad 0 \quad \frac{y}{\sqrt{x^2+y^2}}\right] JH​=[x2+y2​x​0x2+y2​y​]

状态转移方程的雅克比矩阵为：

JA=(1Δ0010001)J_A=\left( \begin{array}{c} 1 & \Delta & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{array} \right) JA​=⎝⎛​100​Δ10​001​⎠⎞​

得到上述矩阵后我们就可以设定初值和噪声，然后根据流程图中的步骤进行迭代计算。

C++代码

代码

飞机高度demo2

飞机在2D空间中飞行，其中x轴是飞机行进的距离，y轴是飞机的高度。该系统可以由以下连续方程表示：

其中mmm是飞机的重量（1000千克）

bxb_ {x}bx​是阻力系数（0.35 N /平方米/秒²）

ppp是提升力（3.92 N /平方米/秒²）

ggg是重力加速度（9.8米/秒²）

uuu输入的电机的推力

计算雅各比矩阵：

即：

设置状态向量的初始估计之后，设置代表状态向量估计误差的协方差的协方差矩阵P。然后，设置协方差Q和R，分别代表过程噪声和测量噪声的协方差矩阵。

C++代码

代码

EKF的不足

EKF存在的主要问题如下：

运动及观察模型用泰勒级数的一阶或二阶展开近似成线性模型，忽略了高阶项，不可避免的引入线性误差，甚至导致滤波器发散。有如下误差补偿方法：

泰勒近似使得状态预测必然存在误差：

A) 补偿状态预测中的误差，附加“人为过程噪声”，即通过增大过程噪声协方差来实现这一点。

B) 扩大状态预测协方差矩阵，用标量加权因子φ>1φ>1φ>1乘状态预测协方差矩阵

C) 利用对角矩阵φ=diag(sqrt(φi)),φi>1φ=diag(sqrt(φ_i)), φ_i>1φ=diag(sqrt(φi​)),φi​>1来乘以状态预测协方差矩阵

其实无论增大过程噪声协方差还是状态预测协方差矩阵，都是为了增大kalman增益，即状态预测是不准的，我要减小一步状态预测在状态更新中的权重。

雅克比矩阵（一阶）及海塞矩阵（二阶）计算困难。二阶EKF的性能要好于一阶的，而二阶以上的性能相比于二阶并没有太大的提高，所以超过二阶以上的EKF一般不采用。但二阶EKF的性能虽好，但计算量大，一般情况下不用

为了解决这一问题，请参见下一期无损卡尔曼滤波~~

参考文献

由浅入深的扩展卡尔曼滤波器教程