Extended Kalman Filter（扩展卡尔曼滤波）是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下，EKF已经成为了非线性系统状态估计的事实标准。本文将简要介绍EKF，并介绍其在无人驾驶多传感器融合上的应用。
KF与EKF
本文假定读者已熟悉KF，若不熟悉请参考卡尔曼滤波简介。
KF与EKF的区别如下：
- 预测未来： x′=Fx+u <script type="math/tex" id="MathJax-Element-1">x'=Fx+u</script>用 x′=f(x,u) <script type="math/tex" id="MathJax-Element-2">x'=f(x,u)</script>代替；其余 F <script type="math/tex" id="MathJax-Element-3">F</script>用Fj<script type="math/tex" id="MathJax-Element-4">F_j</script>代替。
- 修正当下：将状态映射到测量的 Hx′ <script type="math/tex" id="MathJax-Element-5">Hx'</script>用 h(x′) <script type="math/tex" id="MathJax-Element-6">h(x')</script>代替；其余 H <script type="math/tex" id="MathJax-Element-7">H</script>用Hj<script type="math/tex" id="MathJax-Element-8">H_j</script>代替。
其中，非线性函数 f(x,u)，h(x′) <script type="math/tex" id="MathJax-Element-9">f(x,u)，h(x')</script>用非线性得到了更精准的状态预测值、映射后的测量值；线性变换 Fj，Hj <script type="math/tex" id="MathJax-Element-10">F_j，H_j</script>通过线性变换使得变换后的 x，z <script type="math/tex" id="MathJax-Element-11">x，z</script>仍满足高斯分布的假设。
Fj，Hj <script type="math/tex" id="MathJax-Element-12">F_j，H_j</script>计算方式如下：
Fjb=∂f(x,u)∂x=∂h(x′)∂x
<script type="math/tex; mode=display" id="MathJax-Element-13">\begin{split} F_j &= \frac{\partial f(x, u)}{\partial x} \\ b &= \frac{\partial h(x')}{\partial x} \end{split}</script>
为什么要用EKF
KF的假设之一就是高斯分布的 x <script type="math/tex" id="MathJax-Element-14">x</script>预测后仍服从高斯分布，高斯分布的x<script type="math/tex" id="MathJax-Element-15">x</script>变换到测量空间后仍服从高斯分布。可是，假如 F、H <script type="math/tex" id="MathJax-Element-16">F、H</script>是非线性变换，那么上述条件则不成立。
将非线性系统线性化
既然非线性系统不行，那么很自然的解决思路就是将非线性系统线性化。
对于一维系统，采用泰勒一阶展开即可得到：
f(x)≈f(μ)+∂f(μ)∂x(x−μ)
<script type="math/tex; mode=display" id="MathJax-Element-720">f(x) \approx f(\mu) + \frac{ \partial f(\mu)}{\partial x}(x-\mu)</script>
对于多维系统，仍旧采用泰勒一阶展开即可得到：
T(x)≈f(a)+(x−a)TDf(a)
<script type="math/tex; mode=display" id="MathJax-Element-18">T(x) \approx f(a) + (x-a)^T Df(a)</script>
其中， Df(a) <script type="math/tex" id="MathJax-Element-19">Df(a)</script>是Jacobian矩阵。
多传感器融合
lidar与radar
本文将以汽车跟踪为例，目标是知道汽车时刻的状态 x=(px,py,vx,vy) <script type="math/tex" id="MathJax-Element-20">x = (p_x, p_y, v_x, v_y)</script>。已知的传感器有lidar、radar。
- lidar：笛卡尔坐标系。可检测到位置，没有速度信息。其测量值 z=(px,py) <script type="math/tex" id="MathJax-Element-21">z=(p_x, p_y)</script>。
- radar：极坐标系。可检测到距离，角度，速度信息，但是精度较低。其测量值 z=(ρ,ϕ,ρ˙) <script type="math/tex" id="MathJax-Element-22">z=(\rho, \phi, \dot{\rho})</script>，图示如下。
传感器融合步骤
步骤图如上所示，包括：
- 收到第一个测量值，对状态 x <script type="math/tex" id="MathJax-Element-23">x</script>进行初始化。
- 预测未来
- 修正当下
初始化
初始化，指在收到第一个测量值后，对状态x<script type="math/tex" id="MathJax-Element-24">x</script>进行初始化。初始化如下，同时加上对时间的更新。
对于radar来说，
⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢10000100⎤⎦⎥⎥⎥⎥[pxpy]
<script type="math/tex; mode=display" id="MathJax-Element-25">\left[ \begin{matrix} p_x \\ p_y \\ v_x \\ v_y \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 \\ 0 & 1 \\ 0 & 0 \\ 0 & 0 \end{matrix} \right] \left[ \begin{matrix} p_x \\ p_y \end{matrix} \right] </script>
对于radar来说，
⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢ρcosϕρsinϕρ˙cosϕρ˙sinϕ⎤⎦⎥⎥⎥⎥
<script type="math/tex; mode=display" id="MathJax-Element-26">\left[ \begin{matrix} p_x \\ p_y \\ v_x \\ v_y \end{matrix} \right] = \left[ \begin{matrix} \rho \cos{\phi} \\ \rho \sin{\phi} \\ \dot{\rho} \cos{\phi}\\ \dot{\rho} \sin{\phi} \end{matrix} \right] </script>
预测未来
预测主要涉及的公式是：
x′P′=Fx=FPFT+Q
<script type="math/tex; mode=display" id="MathJax-Element-280">\begin{split} x' &= Fx \\ P' &= FPF^T + Q \end{split}</script>
需要求解的有三个变量： F、P、Q <script type="math/tex" id="MathJax-Element-281">F、P、Q</script>。
F <script type="math/tex" id="MathJax-Element-282">F</script>表明了系统的状态如何改变，这里仅考虑线性系统，F易得：
Fx=⎡⎣⎢⎢⎢⎢10000100dt0100dt01⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥
<script type="math/tex; mode=display" id="MathJax-Element-283">Fx = \left[ \begin{matrix} 1 & 0 & dt & 0 \\ 0 & 1 & 0& dt \\ 0 & 0 & 1& 0 \\ 0 & 0 & 0& 1 \end{matrix} \right] \left[ \begin{matrix} p_x \\ p_y \\ v_x \\ v_y \end{matrix} \right]</script>
P <script type="math/tex" id="MathJax-Element-284">P</script>表明了系统状态的不确定性程度，用x<script type="math/tex" id="MathJax-Element-285">x</script>的协方差表示，这里自己指定为：
P=⎡⎣⎢⎢⎢⎢1000010000100000001000⎤⎦⎥⎥⎥⎥
<script type="math/tex; mode=display" id="MathJax-Element-296">P = \left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0& 0 \\ 0 & 0 & 1000& 0 \\ 0 & 0 & 0& 1000 \end{matrix} \right]</script>
Q <script type="math/tex" id="MathJax-Element-297">Q</script>表明了x′=Fx<script type="math/tex" id="MathJax-Element-298">x' = Fx</script>未能刻画的其他外界干扰。本例子使用线性模型，因此加速度变成了干扰项。 x′=Fx <script type="math/tex" id="MathJax-Element-299">x' = Fx</script>中未衡量的额外项目 v <script type="math/tex" id="MathJax-Element-300">v</script>为：
v=⎡⎣⎢⎢⎢⎢⎢⎢⎢axdt22aydt22axdtaydt⎤⎦⎥⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢dt220dt00dt220dt⎤⎦⎥⎥⎥⎥⎥⎥[axay]=Ga
<script type="math/tex; mode=display" id="MathJax-Element-208">v = \left[ \begin{matrix} \frac{a_x dt^2}{2} \\ \frac{a_y dt^2}{2} \\ a_x dt \\ a_y dt \end{matrix} \right] = \left[ \begin{matrix} \frac{dt^2}{2} & 0 \\ 0 & \frac{dt^2}{2} \\ dt & 0 \\ 0 & dt \end{matrix} \right] \left[ \begin{matrix} a_x\\ a_y \end{matrix} \right] = Ga</script>
v <script type="math/tex" id="MathJax-Element-209">v</script>服从高斯分布N(0,Q)<script type="math/tex" id="MathJax-Element-210">N(0, Q)</script>。
Q=E[vvT]=E[GaaTGT]=GE[aaT]GT=G[σ2ax00σ2ay]GT=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢dt44σ2ax0dt32σ2ax00dt44σ2ay0dt32σ2aydt32σ2ax0dt2σ2ax00dt32σ2ay0dt2σ2ay⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥
<script type="math/tex; mode=display" id="MathJax-Element-301">\begin{split} Q &= E[v v^T]= E[Gaa^TG^T] = GE[aa^T]G^T \\ &= G\left[ \begin{matrix} \sigma_{ax}^2 & 0\\ 0 & \sigma_{ay}^2 \end{matrix} \right] G^T \\ &= \left[ \begin{matrix} \frac{{dt}^4}{4} \sigma_{ax}^2 & 0 & \frac{{dt}^3}{2} \sigma_{ax}^2 & 0 \\ 0 & \frac{{dt}^4}{4} \sigma_{ay}^2 & 0 & \frac{{dt}^3}{2} \sigma_{ay}^2 \\ \frac{{dt}^3}{2} \sigma_{ax}^2 & 0 & {dt}^2 \sigma_{ax}^2 & 0 \\ 0 & \frac{{dt}^3}{2} \sigma_{ay}^2 & 0& {dt}^2 \sigma_{ay}^2 \end{matrix} \right]\end{split} </script>
修正当下
lidar
lidar使用了KF。修正当下这里牵涉到的公式主要是：
ySKx′P′=z−Hx=HPHT+R=PHTS−1=x+Ky=(I−KH)P
<script type="math/tex; mode=display" id="MathJax-Element-394">\begin{split} y &= z - Hx \\ S &= HPH^T+R \\ K &= PH^TS^{-1} \\ x' &= x+Ky \\ P' &= (I-KH)P \end{split}</script>
需要求解的有两个变量： H、R <script type="math/tex" id="MathJax-Element-395">H、R</script>。
H <script type="math/tex" id="MathJax-Element-396">H</script>表示了状态空间到测量空间的映射。
Hx=[10010000]⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥
<script type="math/tex; mode=display" id="MathJax-Element-463">Hx = \left[ \begin{matrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 \end{matrix} \right] \left[ \begin{matrix} p_x \\ p_y \\ v_x \\ v_y \end{matrix} \right] </script>
R <script type="math/tex" id="MathJax-Element-464">R</script>表示了测量值的不确定度，一般由传感器的厂家提供，这里lidar参考如下：
Rlaser=[0.0225000.0225]
<script type="math/tex; mode=display" id="MathJax-Element-475">R_{laser} = \left[ \begin{matrix} 0.0225 & 0 \\ 0 & 0.0225 \end{matrix} \right]</script>
radar
radar使用了EKF。修正当下这里牵涉到的公式主要是：
ySKx′P′=z−f(x)=HjPHTj+R=PHTjS−1=x+Ky=(I−KHj)P
<script type="math/tex; mode=display" id="MathJax-Element-587">\begin{split} y &= z - f(x) \\ S &= H_jPH_j^T+R \\ K &= PH_j^TS^{-1} \\ x' &= x+Ky \\ P' &= (I-KH_j)P \end{split}</script>
区别与上面lidar的主要有：
- 状态空间到测量空间的非线性映射 f(x) <script type="math/tex" id="MathJax-Element-588">f(x)</script>
- 非线性映射线性化后的Jacob矩阵
- radar的 Rradar <script type="math/tex" id="MathJax-Element-589">R_{radar}</script>
状态空间到测量空间的非线性映射 f(x) <script type="math/tex" id="MathJax-Element-590">f(x)</script>如下
f(x)=⎡⎣⎢⎢ρϕρ˙⎤⎦⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢p2x+p2y‾‾‾‾‾‾‾√arctanpypxpxvx+pyvyp2x+p2y‾‾‾‾‾‾‾√⎤⎦⎥⎥⎥⎥⎥⎥⎥
<script type="math/tex; mode=display" id="MathJax-Element-645">f(x) = \left[ \begin{matrix} \rho \\ \phi \\ \dot{\rho} \end{matrix} \right] = \left[ \begin{matrix} \sqrt{p_x^2+p_y^2} \\ \arctan{\frac{p_y}{p_x}} \\ \frac{p_x v_x + p_y v_y}{\sqrt{p_x^2+p_y^2} } \end{matrix}\right]</script>
非线性映射线性化后的Jacob矩阵 Hj <script type="math/tex" id="MathJax-Element-646">H_j</script>
Hj=∂f(x)∂x=⎡⎣⎢⎢⎢⎢⎢⎢⎢∂ρ∂px∂ϕ∂px∂ρ˙∂px∂ρ∂py∂ϕ∂py∂ρ˙∂py∂ρ∂vx∂ϕ∂vx∂ρ˙∂vx∂ρ∂vy∂ϕ∂vy∂ρ˙∂vy⎤⎦⎥⎥⎥⎥⎥⎥⎥
<script type="math/tex; mode=display" id="MathJax-Element-718">H_j = \frac{\partial f(x)}{\partial x} = \left[ \begin{matrix} \frac{\partial \rho}{\partial p_x} & \frac{\partial \rho}{\partial p_y} & \frac{\partial \rho}{\partial v_x} & \frac{\partial \rho}{\partial v_y} \\ \frac{\partial \phi}{\partial p_x} & \frac{\partial \phi}{\partial p_y} & \frac{\partial \phi}{\partial v_x} & \frac{\partial \phi}{\partial v_y} \\ \frac{\partial \dot{\rho}}{\partial p_x} & \frac{\partial \dot{\rho}}{\partial p_y} & \frac{\partial \dot{\rho}}{\partial v_x} & \frac{\partial \dot{\rho}}{\partial v_y} \end{matrix} \right]</script>
R <script type="math/tex" id="MathJax-Element-719">R</script>表示了测量值的不确定度，一般由传感器的厂家提供，这里radar参考如下：
Rlaser=⎡⎣⎢⎢0.090000.00090000.09⎤⎦⎥⎥
<script type="math/tex; mode=display" id="MathJax-Element-717">R_{laser} = \left[ \begin{matrix} 0.09 & 0 & 0\\ 0 & 0.0009 & 0 \\ 0 & 0 & 0.09 \end{matrix} \right]</script>
传感器融合实例
多传感器融合的示例如下，需要注意的有：
- lidar和radar的预测部分是完全相同的
- lidar和radar的参数更新部分是不同的，不同的原因是不同传感器收到的测量值是不同的
- 当收到lidar或radar的测量值，依次执行预测、更新步骤
- 当同时收到lidar和radar的测量值，依次执行预测、更新1、更新2步骤
多传感器融合的效果如下图所示，红点和蓝点分别表示radar和lidar的测量位置，绿点代表了EKF经过多传感器融合后获取到的测量位置，取得了较低的RMSE。
73
0
已为社区贡献7条内容
所有评论(0)