传感器融合：基于EKF的Lidar与Radar数据融合
文章目录
- Overview
- State Transition & Measurement Function
- Kalman Filter Algorithm
- Prediction
- State Transition Matrix
- Process Noise Covariance Matrix
- Measurement Update
- Lidar Measurements
- Radar Measurements
- Reference
Overview
- 代码：sensor_fusion_cg/fusion_lidar_radar
State Transition & Measurement Function
State transition function:
x ′ = f ( x ) + ν x^{\prime}=f(x)+\nu x′=f(x)+ν
Measurement function:
z = h ( x ′ ) + ω z=h\left(x^{\prime}\right)+\omega z=h(x′)+ω
其中， f ( x ) f(x) f(x) 和 h ( x ) h(x) h(x) 非线性，通过一阶泰勒展开可被线性化为
f ( x ) ≈ f ( μ ) + ∂ f ( μ ) ∂ x ⎵ F j ( x − μ ) f(x) \approx f(\mu)+\underbrace{\frac{\partial f(\mu)}{\partial x}}_{F_{j}}(x-\mu) f(x)≈f(μ)+Fj ∂x∂f(μ)(x−μ)
h ( x ) ≈ h ( μ ) + ∂ h ( μ ) ∂ x ⎵ H j ( x − μ ) h(x) \approx h(\mu)+\underbrace{\frac{\partial h(\mu)}{\partial x}}_{H_{j}}(x-\mu) h(x)≈h(μ)+Hj ∂x∂h(μ)(x−μ)
Kalman Filter Algorithm
State Prediction:
x ′ = F x + B u + ν {x^{\prime}=F x + Bu + \nu} x′=Fx+Bu+ν
P ′ = F P F T + Q P^{\prime}=F P F^{T}+Q P′=FPFT+Q
Measurement Update:
y = z − H x ′ y=z-H x^{\prime} y=z−Hx′
S = H P ′ H T + R S=H P^{\prime} H^{T}+R S=HP′HT+R
K = P ′ H T S − 1 K=P^{\prime} H^{T} S^{-1} K=P′HTS−1
x = x ′ + K y x=x^{\prime}+K y x=x′+Ky
P = ( I − K H ) P ′ P=(I-K H) P^{\prime} P=(I−KH)P′
Prediction
状态转移方程
x ′ = f ( x ) + ν x^{\prime} = f(x) + \nu x′=f(x)+ν
2D常加速度运动模型 为
{ p x ′ = p x + v x Δ t + a x Δ t 2 2 p y ′ = p y + v y Δ t + a y Δ t 2 2 v x ′ = v x + a x Δ t v y ′ = v y + a y Δ t \left\{\begin{array}{l} {p_{x}^{\prime}=p_{x}+v_{x} \Delta t+\frac{a_{x} \Delta t^{2}}{2}} \\ {p_{y}^{\prime}=p_{y}+v_{y} \Delta t+\frac{a_{y} \Delta t^{2}}{2}} \\ {v_{x}^{\prime}=v_{x}+a_{x} \Delta t} \\ {v_{y}^{\prime}=v_{y}+a_{y} \Delta t} \end{array}\right. ⎩⎪⎪⎪⎨⎪⎪⎪⎧px′=px+vxΔt+2axΔt2py′=py+vyΔt+2ayΔt2vx′=vx+axΔtvy′=vy+ayΔt
写成矩阵形式
( p x ′ p y ′ v x ′ v y ′ ) = ( 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ) ( p x p y v x v y ) + ( a x Δ t 2 2 a y Δ t 2 2 a x Δ t a y Δ t ) \left(\begin{array}{c} {p_{x}^{\prime}} \\ {p_{y}^{\prime}} \\ {v_{x}^{\prime}} \\ {v_{y}^{\prime}} \end{array}\right) = \left(\begin{array}{cccc} {1} & {0} & {\Delta t} & {0} \\ {0} & {1} & {0} & {\Delta t} \\ {0} & {0} & {1} & {0} \\ {0} & {0} & {0} & {1} \end{array}\right) \left(\begin{array}{l} {p_{x}} \\ {p_{y}} \\ {v_{x}} \\ {v_{y}} \end{array}\right) + \left(\begin{array}{c} {\frac{a_{x} \Delta t^{2}}{2}} \\ {\frac{a_{y} \Delta t^{2}}{2}} \\ {a_{x} \Delta t} \\ {a_{y} \Delta t} \end{array}\right) ⎝⎜⎜⎛px′py′vx′vy′⎠⎟⎟⎞=⎝⎜⎜⎛10000100Δt0100Δt01⎠⎟⎟⎞⎝⎜⎜⎛pxpyvxvy⎠⎟⎟⎞+⎝⎜⎜⎛2axΔt22ayΔt2axΔtayΔt⎠⎟⎟⎞
抽象简写为
x ′ = F x + v x^{\prime}=Fx + v x′=Fx+v
其中
v ∼ N ( 0 , Q ) v \sim N(0, Q) v∼N(0,Q)
State Transition Matrix
F = ( 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ) F = \left(\begin{array}{cccc} {1} & {0} & {\Delta t} & {0} \\ {0} & {1} & {0} & {\Delta t} \\ {0} & {0} & {1} & {0} \\ {0} & {0} & {0} & {1} \end{array}\right) F=⎝⎜⎜⎛10000100Δt0100Δt01⎠⎟⎟⎞
Process Noise Covariance Matrix
由上式
ν = ( ν p x ν p y ν v x ν v y ) = ( a x Δ t 2 2 a y Δ t 2 2 a x Δ t a y Δ t ) = ( Δ t 2 2 0 0 Δ t 2 2 Δ t 0 0 Δ t ) ⎵ G ( a x a y ) ⎵ a = G a \nu= \left(\begin{array}{c} {\nu_{p x}} \\ {\nu_{p y}} \\ {\nu_{v x}} \\ {\nu_{v y}} \end{array}\right) = \left(\begin{array}{c} {\frac{a_{x} \Delta t^{2}}{2}} \\ {\frac{a_{y} \Delta t^{2}}{2}} \\ {a_{x} \Delta t} \\ {a_{y} \Delta t} \end{array}\right) = \underbrace{ \left(\begin{array}{cc} {\frac{\Delta t^{2}}{2}} & {0} \\ {0} & {\frac{\Delta t^{2}}{2}} \\ {\Delta t} & {0} \\ {0} & {\Delta t}\end{array}\right)}_{G} \underbrace{\left(\begin{array}{l}{a_{x}} \\ {a_{y}}\end{array}\right)}_{a} = Ga ν=⎝⎜⎜⎛νpxνpyνvxνvy⎠⎟⎟⎞=⎝⎜⎜⎛2axΔt22ayΔt2axΔtayΔt⎠⎟⎟⎞=G ⎝⎜⎜⎛2Δt20Δt002Δt20Δt⎠⎟⎟⎞a (axay)=Ga
根据 v ∼ N ( 0 , Q ) v \sim N(0, Q) v∼N(0,Q)
Q = E [ ν ν T ] = E [ G a a T G T ] Q=E\left[\nu \nu^{T}\right]=E\left[G a a^{T} G^{T}\right] Q=E[ννT]=E[GaaTGT]
因为 G G G 不包含随机变量，将其移出
Q = G E [ a a T ] G T = G ( σ a x 2 σ a x y σ a x y σ a y 2 ) G T = G Q ν G T Q = G E\left[a a^{T}\right] G^{T} = G \left(\begin{array}{cc} {\sigma_{a x}^{2}} & {\sigma_{a x y}} \\ {\sigma_{a x y}} & {\sigma_{a y}^{2}} \end{array}\right) G^{T} = G Q_{\nu} G^{T} Q=GE[aaT]GT=G(σax2σaxyσaxyσay2)GT=GQνGT
a x a_x ax 和 a y a_y ay 假设不相关，则
Q ν = ( σ a x 2 σ a x y σ a x y σ a y 2 ) = ( σ a x 2 0 0 σ a y 2 ) Q_{\nu} = \left(\begin{array}{cc} {\sigma_{a x}^{2}} & {\sigma_{a x y}} \\ {\sigma_{a x y}} & {\sigma_{a y}^{2}} \end{array}\right) = \left(\begin{array}{cc} {\sigma_{a x}^{2}} & {0} \\ {0} & {\sigma_{a y}^{2}} \end{array}\right) Qν=(σax2σaxyσaxyσay2)=(σax200σay2)
最终
Q = G Q ν G T = ( Δ t 4 4 σ a x 2 0 Δ t 3 2 σ a x 2 0 0 Δ t 4 4 σ a y 2 0 Δ t 3 2 σ a y 2 Δ t 3 2 σ a x 2 0 Δ t 2 σ a x 2 0 0 Δ t 3 2 σ a y 2 0 Δ t 2 σ a y 2 ) Q = G Q_{\nu} G^{T} = \left(\begin{array}{cccc} {\frac{\Delta t^{4}}{4} \sigma_{a x}^{2}} & {0} & {\frac{\Delta t^{3}}{2} \sigma_{a x}^{2}} & {0} \\ {0} & {\frac{\Delta t^{4}}{4} \sigma_{a y}^{2}} & {0} & {\frac{\Delta t^{3}}{2} \sigma_{a y}^{2}} \\ {\frac{\Delta t^{3}}{2} \sigma_{a x}^{2}} & {0} & {\Delta t^{2} \sigma_{a x}^{2}} & {0} \\ {0} & {\frac{\Delta t^{3}}{2} \sigma_{a y}^{2}} & {0} & {\Delta t^{2} \sigma_{a y}^{2}} \end{array}\right) Q=GQνGT=⎝⎜⎜⎜⎛4Δt4σax202Δt3σax2004Δt4σay202Δt3σay22Δt3σax20Δt2σax2002Δt3σay20Δt2σay2⎠⎟⎟⎟⎞
Measurement Update
测量方程
z = h ( x ′ ) + ω z = h(x^{\prime}) + \omega z=h(x′)+ω
Lidar Measurements
Lidar测量方程
z = ( p x p y ) = ( 1 0 0 0 0 1 0 0 ) ( p x ′ p y ′ v x ′ v y ′ ) z = \left(\begin{array}{c}{p_{x}} \\ {p_{y}}\end{array}\right) = \left(\begin{array}{cccc} {1} & {0} & {0} & {0} \\ {0} & {1} & {0} & {0} \end{array}\right) \left(\begin{array}{c} {p_{x}^{\prime}} \\ {p_{y}^{\prime}} \\ {v_{x}^{\prime}} \\ {v_{y}^{\prime}} \end{array}\right) z=(pxpy)=(10010000)⎝⎜⎜⎛px′py′vx′vy′⎠⎟⎟⎞
简写为
z = H x ′ + ω s . t . ω ∼ N ( 0 , R ) z = H x^{\prime} + \omega \quad s.t. \quad \omega \sim N(0,R) z=Hx′+ωs.t.ω∼N(0,R)
则 Measurement Jacobian Matrix
H = ( 1 0 0 0 0 1 0 0 ) H = \left(\begin{array}{cccc} {1} & {0} & {0} & {0} \\ {0} & {1} & {0} & {0} \end{array}\right) H=(10010000)
Lidar Measurement Noise Covariance Matrix
R = E [ ω ω T ] = ( σ p x 2 0 0 σ p y 2 ) R = E\left[\omega \omega^{T}\right] = \left(\begin{array}{cc} {\sigma_{p x}^{2}} & {0} \\ {0} & {\sigma_{p y}^{2}} \end{array}\right) R=E[ωωT]=(σpx200σpy2)
Radar Measurements
Radar测量方程
z = ( ρ φ ρ ˙ ) = h ( x ′ ) = ( p x ′ 2 + p y ′ 2 arctan ( p y ′ / p x ′ ) p x ′ v x ′ + p y v y ′ p x ′ 2 + p y ′ 2 ) z = \left(\begin{array}{l} {\rho} \\ {\varphi} \\ {\dot{\rho}} \end{array}\right) = h(x^{\prime}) = \left(\begin{array}{c} {\sqrt{p_{x}^{\prime 2}+p_{y}^{\prime 2}}} \\ {\arctan \left(p_{y}^{\prime} / p_{x}^{\prime}\right)} \\ {\frac{p_{x}^{\prime} v_{x}^{\prime}+p_{y} v_{y}^{\prime}}{\sqrt{p_{x}^{\prime 2}+p_{y}^{\prime 2}}}} \end{array}\right) z=⎝⎛ρφρ˙⎠⎞=h(x′)=⎝⎜⎜⎛px′2+py′2arctan(py′/px′)px′2+py′2px′vx′+pyvy′⎠⎟⎟⎞
- range ρ \rho ρ: the radial distance from the origin to our pedestrian
- bearing φ \varphi φ: the angle between the ray and x direction
- range rate ρ ˙ \dot{\rho} ρ˙: known as Doppler or radial velocity is the velocity along this ray
通过一阶泰勒展开将 h ( x ′ ) h(x^{\prime}) h(x′) 在 μ = 0 \mu = 0 μ=0 处线性化
h ( x ′ ) = H x ′ h(x^{\prime}) = H x^{\prime} h(x′)=Hx′
简写为
z = H x ′ + ω s . t . ω ∼ N ( 0 , R ) z = H x^{\prime} + \omega \quad s.t. \quad \omega \sim N(0,R) z=Hx′+ωs.t.ω∼N(0,R)
其中，Measurement Jacobian Matrix
H = [ ∂ ρ ∂ p x ∂ ρ ∂ p y ∂ ρ ∂ v x ∂ ρ ∂ v y ∂ φ ∂ p x ∂ φ ∂ p y ∂ φ ∂ v x ∂ φ ∂ v y ∂ ρ ˙ ∂ p x ∂ ρ ∂ p y ∂ ρ ˙ ∂ v x ∂ ρ ∂ v y ] = [ p x p x 2 + p y 2 p y p x 2 + p y 2 0 0 − p y p x 2 + p y 2 p x p x 2 + p y 2 0 0 p y ( v x p y − v y p x ) ( p x 2 + p y 2 ) 3 / 2 p x ( v y p x − v x p y ) ( p x 2 + p y 2 ) 3 / 2 p x p x 2 + p y 2 p y p x 2 + p y 2 ] H = \left[\begin{array}{llll} {\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 \varphi}{\partial p_{x}}} & {\frac{\partial \varphi}{\partial p_{y}}} & {\frac{\partial \varphi}{\partial v_{x}}} & {\frac{\partial \varphi}{\partial v_{y}}} \\ {\frac{\partial \dot{\rho}}{\partial p_{x}}} & {\frac{\partial \rho}{\partial p_{y}}} & {\frac{\partial \dot{\rho}}{\partial v_{x}}} & {\frac{\partial \rho}{\partial v_{y}}} \end{array}\right] = \left[\begin{array}{cccc} {\frac{p_{x}}{\sqrt{p_{x}^{2}+p_{y}^{2}}}} & {\frac{p_{y}}{\sqrt{p_{x}^{2}+p_{y}^{2}}}} & {0} & {0} \\ {-\frac{p_{y}}{p_{x}^{2}+p_{y}^{2}}} & {\frac{p_{x}}{p_{x}^{2}+p_{y}^{2}}} & {0} & {0} \\ {\frac{p_{y}\left(v_{x} p_{y}-v_{y} p_{x}\right)}{\left(p_{x}^{2}+p_{y}^{2}\right)^{3 / 2}}} & {\frac{p_{x}\left(v_{y} p_{x}-v_{x} p_{y}\right)}{\left(p_{x}^{2}+p_{y}^{2}\right)^{3 / 2}}} & {\frac{p_{x}}{\sqrt{p_{x}^{2}+p_{y}^{2}}}} & {\frac{p_{y}}{\sqrt{p_{x}^{2}+p_{y}^{2}}}} \end{array}\right] H=⎣⎢⎡∂px∂ρ∂px∂φ∂px∂ρ˙∂py∂ρ∂py∂φ∂py∂ρ∂vx∂ρ∂vx∂φ∂vx∂ρ˙∂vy∂ρ∂vy∂φ∂vy∂ρ⎦⎥⎤=⎣⎢⎢⎡px2+py2px−px2+py2py(px2+py2)3/2py(vxpy−vypx)px2+py2pypx2+py2px(px2+py2)3/2px(vypx−vxpy)00px2+py2px00px2+py2py⎦⎥⎥⎤
Radar Measurement Noise Covariance Matrix
R = ( σ ρ 2 0 0 0 σ φ 2 0 0 0 σ ρ ˙ 2 ) R = \left(\begin{array}{ccc} {\sigma_{\rho}^{2}} & {0} & {0} \\ {0} & {\sigma_{\varphi}^{2}} & {0} \\ {0} & {0} & {\sigma_{\dot{\rho}}^{2}} \end{array}\right) R=⎝⎛σρ2000σφ2000σρ˙2⎠⎞
Reference
- Self-Driving Car ND - Sensor Fusion - Extended Kalman Filters