卡尔曼运动模型公式推导CTRV+CTRA
#! https://zhuanlan.zhihu.com/p/389589611
卡尔曼运动模型公式推导CTRV+CTRA
主要是EKF的CTRV、CTRA两个运动模型的公式推导，以及困扰我很久的Q矩阵推导，一直不明白为什么要用 Δ t \Delta t Δt来设置Q矩阵。直接看公式会比较枯燥，建议推导一下。
1 扩展卡尔曼滤波EKF
解决非线性问题，如极坐标系的雷达，观测到的是径向距离和角度，这个时候的观测矩阵 H H H就是非线性的函数。（极坐标系可以转笛卡尔坐标系，Apollo融合跟踪里面对雷达物体用的是笛卡尔坐标系做KF）
EKF和KF的区别主要是状态转换模型和观测模型可以是非线性的，可以使用泰勒展开式替换为线性函数，两个协方差矩阵 P P P和 H H H要使用雅各比矩阵计算每个状态量的一阶偏导。
预测
x ^ k ∣ k − 1 = f ( x k − 1 , u k , 0 ) \hat x_{k|k-1} = f(x_{k-1},u_k,0) x^k∣k−1=f(xk−1,uk,0)
P k ∣ k − 1 = J F P k − 1 ∣ k − 1 J F T + Q k P_{k|k-1} = J_FP_{k-1|k-1}J^T_F + Q_k Pk∣k−1=JFPk−1∣k−1JFT+Qk
使用Jacobians矩阵更新模型
J F = ∂ f ∂ x ∣ x ^ k − 1 ∣ k − 1 , u k J_F = \frac {\partial f}{\partial x} |_{\hat x_{k-1|k-1}, u_k} JF=∂x∂f∣x^k−1∣k−1,uk
J H = ∂ h ∂ x ∣ x ^ k ∣ k − 1 J_H = \frac {\partial h}{\partial x} |_{\hat x_{k|k-1}} JH=∂x∂h∣x^k∣k−1
更新
y ^ k = z k − h ( x ^ k ∣ k − 1 , 0 ) \hat y_k = z_k - h(\hat x_{k|k-1},0) y^k=zk−h(x^k∣k−1,0)
S k = J H P k ∣ k − 1 H k T + R k S_k = J_H P_{k|k-1} H^T_k + R_k Sk=JHPk∣k−1HkT+Rk
K k = P k ∣ k − 1 J H T S k − 1 K_k = P_{k|k-1}J^T_H S_k^{-1} Kk=Pk∣k−1JHTSk−1
x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ^ \hat x_{k|k} = \hat x_{k|k-1} + K_k \hat y x^k∣k=x^k∣k−1+Kky^
P k ∣ k = ( I − K k J H ) P k ∣ k − 1 P_{k|k} = (I - K_k J_H) P_{k|k-1} Pk∣k=(I−KkJH)Pk∣k−1
2 恒定转弯率和速度模型(Constant Turn Rate and Velocity,CTRV)
状态量为：
X = [ x y v θ ω ] X = \begin{bmatrix}x \\ y \\ v \\ \theta \\ \omega \end{bmatrix} X=⎣⎢⎢⎢⎢⎡xyvθω⎦⎥⎥⎥⎥⎤
状态方程为：
X k + 1 = X k + [ ∫ t k t k + 1 v ( t ) c o s ( θ ( t ) ) ∫ t k t k + 1 v ( t ) s i n ( θ ( t ) ) 0 w Δ t 0 ] X_{k+1} = X_k + \begin{bmatrix} {\int^{t_{k+1}}_{t_k} v(t)cos(\theta(t))} \\ {\int^{t_{k+1}}_{t_k} v(t)sin(\theta(t))} \\ 0 \\ w \Delta t \\ 0 \end{bmatrix} Xk+1=Xk+⎣⎢⎢⎢⎢⎢⎡∫tktk+1v(t)cos(θ(t))∫tktk+1v(t)sin(θ(t))0wΔt0⎦⎥⎥⎥⎥⎥⎤
v ( t ) = v k v(t)=v_k v(t)=vk为常量,角速度 w w w也是常量，和时间无关。后面公式有点多，速度项可能有k下标，或者没有， v k , v v_k,v vk,v均是一个意思。
积分化简项为 w ≠ 0 : w \neq 0: w=0:
note：对于 w = 0 w = 0 w=0时，以下的公式都是可以简化的，简化公式不做阐述，可自行推导。或者在代码中也可以使用 w = 0.0001 w = 0.0001 w=0.0001一个极小数来赋值，解决被除无限大的问题
∫ t k t k + 1 v ( t ) c o s ( θ ( t ) ) d t = v ( t ) ∫ t k t k + 1 c o s ( θ ( t ) ) d t = v k ∫ t k t k + 1 c o s ( θ k + w ( t − t k ) ) d t = v k w [ s i n ( w Δ t + θ ) − s i n ( θ ) ] {\int^{t_{k+1}}_{t_k} v(t)cos(\theta(t))dt} = {v(t) \int^{t_{k+1}}_{t_k} cos(\theta(t))dt} = {v_k \int^{t_{k+1}}_{t_k} cos(\theta_k + w (t-t_k))dt} = \frac {v_k} {w} {[sin(w \Delta t + \theta) - sin(\theta)]} ∫tktk+1v(t)cos(θ(t))dt=v(t)∫tktk+1cos(θ(t))dt=vk∫tktk+1cos(θk+w(t−tk))dt=wvk[sin(wΔt+θ)−sin(θ)]
∫ t k t k + 1 v ( t ) s i n ( θ ( t ) ) d t = v ( t ) ∫ t k t k + 1 s i n ( θ ( t ) ) d t = v k ∫ t k t k + 1 s i n ( θ k + w ( t − t k ) ) d t = v k w [ − c o s ( w Δ t + θ ) + c o s ( θ ) ] {\int^{t_{k+1}}_{t_k} v(t)sin(\theta(t))dt}= {v(t) \int^{t_{k+1}}_{t_k} sin(\theta(t))dt} = {v_k \int^{t_{k+1}}_{t_k} sin(\theta_k + w (t-t_k))dt} = \frac {v_k} {w} {[-cos(w \Delta t + \theta) + cos(\theta)]} ∫tktk+1v(t)sin(θ(t))dt=v(t)∫tktk+1sin(θ(t))dt=vk∫tktk+1sin(θk+w(t−tk))dt=wvk[−cos(wΔt+θ)+cos(θ)]
因此最终的状态方程为：
X k + 1 = X k + [ v k w [ s i n ( w Δ t + θ ) − s i n ( θ ) ] v k w [ − c o s ( w Δ t + θ ) + c o s ( θ ) ] 0 w Δ t 0 ] = [ x k + v k w [ s i n ( w Δ t + θ ) − s i n ( θ ) ] y k + v k w [ − c o s ( w Δ t + θ ) + c o s ( θ ) ] v k w Δ t + θ w ] X_{k+1} = X_k + \begin{bmatrix} {\frac {v_k} {w} {[sin(w \Delta t + \theta) - sin(\theta)]}}\\ {\frac {v_k} {w} {[-cos(w \Delta t + \theta) + cos(\theta)]}} \\ 0 \\ w \Delta t \\ 0 \end{bmatrix} = \begin{bmatrix} {x_k + \frac {v_k} {w} {[sin(w \Delta t + \theta) - sin(\theta)]}}\\ {y_k + \frac {v_k} {w} {[-cos(w \Delta t + \theta) + cos(\theta)]}} \\ v_k \\ w \Delta t + \theta \\ w \end{bmatrix} Xk+1=Xk+⎣⎢⎢⎢⎢⎡wvk[sin(wΔt+θ)−sin(θ)]wvk[−cos(wΔt+θ)+cos(θ)]0wΔt0⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡xk+wvk[sin(wΔt+θ)−sin(θ)]yk+wvk[−cos(wΔt+θ)+cos(θ)]vkwΔt+θw⎦⎥⎥⎥⎥⎤
对状态方程的的每个状态量( x , y , v , θ , w x,y,v,\theta ,w x,y,v,θ,w)求偏导得到雅各比矩阵 J F J_F JF：
J F = [ 1 0 1 w [ s i n ( w Δ t + θ ) − s i n ( θ ) ] v k w [ c o s ( w Δ t + θ ) − c o s ( θ ) ] v k Δ t w c o s ( w Δ t + θ ) − v k w 2 [ s i n ( w Δ t + θ ) − s i n ( θ ) ] 0 1 1 w [ − c o s ( w Δ t + θ ) + c o s ( θ ) ] v k w [ s i n ( w Δ t + θ ) − s i n ( θ ) ] v k Δ t w s i n ( w Δ t + θ ) − v k w 2 [ − c o s ( w Δ t + θ ) + c o s ( θ ) ] 0 0 1 0 0 0 0 0 1 Δ t 0 0 0 0 1 ] J_F = \begin{bmatrix} 1 & 0 & {\frac 1{w} {[sin(w \Delta t + \theta) - sin(\theta)]}} & {\frac {v_k}{w} {[cos(w \Delta t + \theta) - cos(\theta)]}} & {\frac {v_k \Delta t}{w} cos(w \Delta t + \theta) - \frac {v_k} {w^2} {[sin(w \Delta t + \theta) - sin(\theta)]}} \\ 0 & 1 & {\frac 1{w} {[-cos(w \Delta t + \theta) + cos(\theta)]}} & {\frac {v_k}{w} {[sin(w \Delta t + \theta) - sin(\theta)]}} & {\frac {v_k \Delta t}{w} sin(w \Delta t + \theta) - \frac {v_k} {w^2} {[-cos(w \Delta t + \theta) + cos(\theta)]}} \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} JF=⎣⎢⎢⎢⎢⎡1000001000w1[sin(wΔt+θ)−sin(θ)]w1[−cos(wΔt+θ)+cos(θ)]100wvk[cos(wΔt+θ)−cos(θ)]wvk[sin(wΔt+θ)−sin(θ)]010wvkΔtcos(wΔt+θ)−w2vk[sin(wΔt+θ)−sin(θ)]wvkΔtsin(wΔt+θ)−w2vk[−cos(wΔt+θ)+cos(θ)]0Δt1⎦⎥⎥⎥⎥⎤
对于观测矩阵，如果是线性的，不用做计算。如果是非线性的，同样可以使用求偏导数的方式计算出雅各比矩阵 J H J_H JH。
假设传感器是毫米波雷达观，且观测量是:
z k = [ ρ k θ k ρ ˙ k ] = [ x k 2 + y k 2 a t a n 2 ( y k , x k ) v x x k + v y y k x k 2 + y k 2 ] = [ x k 2 + y k 2 a t a n 2 ( y k , x k ) x k v k c o s θ + y k v k s i n θ x k 2 + y k 2 ] z_k = \begin{bmatrix} \rho_k \\ \theta_k \\ \dot \rho_k \end{bmatrix} = \begin{bmatrix} \sqrt {x^2_k + y^2_k} \\ atan2(y_k,x_k) \\ \frac{v_x x_k + v_y y_k}{\sqrt {x^2_k + y^2_k}} \end{bmatrix} = \begin{bmatrix} \sqrt {x^2_k + y^2_k} \\ atan2(y_k,x_k) \\ \frac{x_k v_k cos\theta + y_k v_k sin\theta}{\sqrt {x^2_k + y^2_k}} \end{bmatrix} zk=⎣⎡ρkθkρ˙k⎦⎤=⎣⎢⎡xk2+yk2atan2(yk,xk)xk2+yk2vxxk+vyyk⎦⎥⎤=⎣⎢⎡xk2+yk2atan2(yk,xk)xk2+yk2xkvkcosθ+ykvksinθ⎦⎥⎤
上式对状态方程的的每个状态量( x , y , v , θ , w x,y,v,\theta ,w x,y,v,θ,w)求偏导得到雅各比矩阵，即观测矩阵 J H J_H JH为：
J H = [ x k x k 2 + y k 2 y k x k 2 + y k 2 0 0 0 − y k x k 2 + y k 2 x k x k 2 + y k 2 0 0 0 y k ( v k c o s θ y k − v k s i n θ x k ) ( x k 2 + y k 2 ) 3 2 x k ( v k s i n θ x k − v k c o s θ y k ) ( x k 2 + y k 2 ) 3 2 ( x k c o s θ + y k s i n θ ) x k 2 + y k 2 ( v k c o s θ y k − v k s i n θ x k ) x k 2 + y k 2 0 ] J_H = \begin{bmatrix} \frac{x_k}{\sqrt {x^2_k + y^2_k}} & \frac{y_k}{\sqrt {x^2_k + y^2_k}} & 0 & 0 & 0 \\ -\frac{y_k}{x^2_k + y^2_k} & \frac{x_k}{x^2_k + y^2_k} & 0 & 0 & 0 \\ \frac{y_k(v_k cos\theta y_k - v_k sin\theta x_k)}{{(x^2_k + y^2_k)^{\frac 32}}} & \frac{x_k( v_k sin\theta x_k - v_k cos\theta y_k)}{{(x^2_k + y^2_k)^{\frac 32}}} & \frac{(x_kcos\theta + y_ksin\theta)}{\sqrt{x_k^2 + y_k^2}} & \frac{(v_k cos\theta y_k - v_k sin\theta x_k)}{\sqrt{x_k^2 + y_k^2}} & 0 \end{bmatrix} JH=⎣⎢⎢⎡xk2+yk2xk−xk2+yk2yk(xk2+yk2)23yk(vkcosθyk−vksinθxk)xk2+yk2ykxk2+yk2xk(xk2+yk2)23xk(vksinθxk−vkcosθyk)00xk2+yk2(xkcosθ+yksinθ)00xk2+yk2(vkcosθyk−vksinθxk)000⎦⎥⎥⎤
补充，另外提一下cv模型：
对于自动驾驶常用CV模型的状态量：
X = [ x y v x v y ] X = \begin{bmatrix}x \\ y \\ v_x \\ v_y \end{bmatrix} X=⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
其状态转移矩阵是线性的就不用求雅各比矩阵了，如果观测是radar: [ ρ , θ , ρ ˙ ] [\rho,\theta, \dot \rho] [ρ,θ,ρ˙]的话，其对应的观测转移矩阵 J H J_H JH为：
J H = [ x k x k 2 + y k 2 y k x k 2 + y k 2 0 0 − y k x k 2 + y k 2 x k x k 2 + y k 2 0 0 y k ( v x y k − v y x k ) ( x k 2 + y k 2 ) 3 2 x k ( v y x k − v x y k ) ( x k 2 + y k 2 ) 3 2 x k x k 2 + y k 2 y k x k 2 + y k 2 ] J_H = \begin{bmatrix} \frac{x_k}{\sqrt {x^2_k + y^2_k}} & \frac{y_k}{\sqrt {x^2_k + y^2_k}} & 0 & 0 \\ -\frac{y_k}{x^2_k + y^2_k} & \frac{x_k}{x^2_k + y^2_k} & 0 & 0 \\ \frac{y_k(v_x y_k - v_y x_k)}{{(x^2_k + y^2_k)^{\frac 32}}} & \frac{x_k(v_y x_k - v_x y_k)}{{(x^2_k + y^2_k)^{\frac 32}}} & \frac{x_k}{\sqrt {x^2_k + y^2_k}} & \frac{y_k}{\sqrt {x^2_k + y^2_k}} \end{bmatrix} JH=⎣⎢⎢⎡xk2+yk2xk−xk2+yk2yk(xk2+yk2)23yk(vxyk−vyxk)xk2+yk2ykxk2+yk2xk(xk2+yk2)23xk(vyxk−vxyk)00xk2+yk2xk00xk2+yk2yk⎦⎥⎥⎤
3 恒定转弯率和加速度模型(Constant Turn Rate and Acceleration,CTRA)
公式量太大，用Python导出的公式直接粘贴了。
状态量为：
X k = [ x y θ v w a ] X_k= \begin{bmatrix}x \\ y \\ \theta \\ v \\ w \\ a \end{bmatrix} Xk=⎣⎢⎢⎢⎢⎢⎢⎡xyθvwa⎦⎥⎥⎥⎥⎥⎥⎤
状态方程为：
X k + 1 = X k + [ ∫ t k t k + 1 v ( t ) c o s ( θ ( t ) ) ∫ t k t k + 1 v ( t ) s i n ( θ ( t ) ) w Δ t a Δ t 0 0 ] X_{k+1} = X_k + \begin{bmatrix} {\int^{t_{k+1}}_{t_k} {v(t)}cos(\theta(t))} \\ {\int^{t_{k+1}}_{t_k} v(t)sin(\theta(t))} \\ w \Delta t \\ a \Delta t \\ 0 \\ 0 \end{bmatrix} Xk+1=Xk+⎣⎢⎢⎢⎢⎢⎢⎢⎡∫tktk+1v(t)cos(θ(t))∫tktk+1v(t)sin(θ(t))wΔtaΔt00⎦⎥⎥⎥⎥⎥⎥⎥⎤
其中 a a a和 w w w为常量,和时间无关， Δ t = t k + 1 − t k \Delta t = t_{k+1}-t_k Δt=tk+1−tk， ω ≠ 0 \omega \neq 0 ω=0，积分化简为：
x k x_k xk项积分：
∫ t k t k + 1 v ( t ) c o s ( θ ( t ) ) d t = ∫ t k t k + 1 ( v k + a ( t − t k ) ) c o s ( θ k + w ( t − t k ) ) d t {\int^{t_{k+1}}_{t_k} v(t)cos(\theta(t))}dt = {\int^{t_{k+1}}_{t_k} (v_k + a(t-t_k))cos(\theta_k + w (t-t_k))} dt ∫tktk+1v(t)cos(θ(t))dt=∫tktk+1(vk+a(t−tk))cos(θk+w(t−tk))dt
= ∫ t k t k + 1 v k c o s ( θ k + w ( t − t k ) ) d t + ∫ t k t k + 1 a ( t − t k ) c o s ( θ k + w ( t − t k ) ) d t = v k ω [ s i n ( w Δ t + θ ) − s i n ( θ ) ] + ∫ t k t k + 1 a ( t − t k ) c o s ( θ k + w ( t − t k ) ) d t ={\int^{t_{k+1}}_{t_k} v_k cos(\theta_k + w (t-t_k))} dt + {\int^{t_{k+1}}_{t_k} a(t-t_k)cos(\theta_k + w (t-t_k))}dt = \frac {v_k} {\omega} {[sin(w \Delta t + \theta) - sin(\theta)]} + {\int^{t_{k+1}}_{t_k} a(t-t_k)cos(\theta_k + w (t-t_k))}dt =∫tktk+1vkcos(θk+w(t−tk))dt+∫tktk+1a(t−tk)cos(θk+w(t−tk))dt=ωvk[sin(wΔt+θ)−sin(θ)]+∫tktk+1a(t−tk)cos(θk+w(t−tk))dt
对 ∫ t k t k + 1 a ( t − t k ) c o s ( θ k + w ( t − t k ) ) d t {\int^{t_{k+1}}_{t_k} a(t-t_k)cos(\theta_k + w (t-t_k))}dt ∫tktk+1a(t−tk)cos(θk+w(t−tk))dt，令 t − t k = x ∈ ( 0 , Δ t ) t-t_k = x \in (0,\Delta t) t−tk=x∈(0,Δt),则 d t = d x dt = dx dt=dx
∫ t k t k + 1 a ( t − t k ) c o s ( θ k + w ( t − t k ) ) d t = a ∫ 0 Δ t x c o s ( θ k + w x ) d x = a w 2 [ w x s i n ( w x + θ k ) + c o s ( w x + θ k ) ] ∣ 0 Δ t = a w 2 [ c o s ( w Δ t + θ k ) − c o s ( θ k ) ] + a Δ t s i n ( w Δ t + θ k ) w {\int^{t_{k+1}}_{t_k} a(t-t_k)cos(\theta_k + w (t-t_k))}dt = {a \int^{\Delta t}_{0} xcos(\theta_k + w x)}dx = \frac a{w^2}[wxsin(wx+\theta_k) + cos(wx+\theta_k)]|^{\Delta t}_0 = \frac a{w^2}[cos(w \Delta t + \theta_k) -cos(\theta_k)] + a \Delta t \frac {sin(w\Delta t + \theta_k)}{w} ∫tktk+1a(t−tk)cos(θk+w(t−tk))dt=a∫0Δtxcos(θk+wx)dx=w2a[wxsin(wx+θk)+cos(wx+θk)]∣0Δt=w2a[cos(wΔt+θk)−cos(θk)]+aΔtwsin(wΔt+θk)
所以合并为：
∫ t k t k + 1 v ( t ) c o s ( θ ( t ) ) d t = 1 w 2 [ ( v k w + a w Δ t ) s i n ( θ k + w Δ t ) + a c o s ( θ k + w Δ t ) − v k w s i n ( θ k ) − a c o s ( θ k ) ] {\int^{t_{k+1}}_{t_k} v(t)cos(\theta(t))}dt = \frac 1{w^2} [(v_kw +aw\Delta t)sin(\theta_k + w\Delta t) + acos(\theta_k+w\Delta t) -v_kwsin(\theta_k)-acos(\theta_k) ] ∫tktk+1v(t)cos(θ(t))dt=w21[(vkw+awΔt)sin(θk+wΔt)+acos(θk+wΔt)−vkwsin(θk)−acos(θk)]
y k y_k yk项积分同上：
∫ t k t k + 1 v ( t ) s i n ( θ ( t ) ) d t = ∫ t k t k + 1 ( v k + a ( t − t k ) ) c o s ( θ k + w ( t − t k ) ) d t {\int^{t_{k+1}}_{t_k} v(t)sin(\theta(t))dt}= {\int^{t_{k+1}}_{t_k} (v_k + a(t-t_k))cos(\theta_k + w (t-t_k))dt} ∫tktk+1v(t)sin(θ(t))dt=∫tktk+1(vk+a(t−tk))cos(θk+w(t−tk))dt
因此最终的状态方程为( T = Δ t T=\Delta t T=Δt)：
X = [ x + − w v sin ( θ ) − a cos ( θ ) + a cos ( T w + θ ) + ( T w a + w v ) sin ( T w + θ ) w 2 y + w v cos ( θ ) − a sin ( θ ) + a sin ( T w + θ ) + ( − T w a − w v ) cos ( T w + θ ) w 2 T w + θ T a + v w a ] X = \displaystyle \left[\begin{matrix}x + \frac{- w v \sin{\left(\theta \right)} - a \cos{\left(\theta \right)} + a \cos{\left(T w + \theta \right)} + \left(T w a + w v\right) \sin{\left(T w + \theta \right)}}{w^{2}}\\y + \frac{w v \cos{\left(\theta \right)} - a \sin{\left(\theta \right)} + a \sin{\left(T w + \theta \right)} + \left(- T w a - w v\right) \cos{\left(T w + \theta \right)}}{w^{2}}\\T w + \theta\\T a + v\\w\\a\end{matrix}\right] X=⎣⎢⎢⎢⎢⎢⎢⎡x+w2−wvsin(θ)−acos(θ)+acos(Tw+θ)+(Twa+wv)sin(Tw+θ)y+w2wvcos(θ)−asin(θ)+asin(Tw+θ)+(−Twa−wv)cos(Tw+θ)Tw+θTa+vwa⎦⎥⎥⎥⎥⎥⎥⎤
对状态方程的的每个状态量( x , y , v , θ , w x,y,v,\theta ,w x,y,v,θ,w)求偏导得到雅各比矩阵 J F J_F JF：
J F = [ 1 0 − w v cos ( θ ) + a sin ( θ ) − a sin ( T w + θ ) + ( T w a + w v ) cos ( T w + θ ) w 2 − w sin ( θ ) + w sin ( T w + θ ) w 2 − T a sin ( T w + θ ) + T ( T w a + w v ) cos ( T w + θ ) − v sin ( θ ) + ( T a + v ) sin ( T w + θ ) w 2 − 2 ( − w v sin ( θ ) − a cos ( θ ) + a cos ( T w + θ ) + ( T w a + w v ) sin ( T w + θ ) ) w 3 T w sin ( T w + θ ) − cos ( θ ) + cos ( T w + θ ) w 2 0 1 − w v sin ( θ ) − a cos ( θ ) + a cos ( T w + θ ) − ( − T w a − w v ) sin ( T w + θ ) w 2 w cos ( θ ) − w cos ( T w + θ ) w 2 T a cos ( T w + θ ) − T ( − T w a − w v ) sin ( T w + θ ) + v cos ( θ ) + ( − T a − v ) cos ( T w + θ ) w 2 − 2 ( w v cos ( θ ) − a sin ( θ ) + a sin ( T w + θ ) + ( − T w a − w v ) cos ( T w + θ ) ) w 3 − T w cos ( T w + θ ) − sin ( θ ) + sin ( T w + θ ) w 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 ] J_F = \displaystyle \left[\begin{matrix}1 & 0 & \frac{- w v \cos{\left(\theta \right)} + a \sin{\left(\theta \right)} - a \sin{\left(T w + \theta \right)} + \left(T w a + w v\right) \cos{\left(T w + \theta \right)}}{w^{2}} & \frac{- w \sin{\left(\theta \right)} + w \sin{\left(T w + \theta \right)}}{w^{2}} & \frac{- T a \sin{\left(T w + \theta \right)} + T \left(T w a + w v\right) \cos{\left(T w + \theta \right)} - v \sin{\left(\theta \right)} + \left(T a + v\right) \sin{\left(T w + \theta \right)}}{w^{2}} - \frac{2 \left(- w v \sin{\left(\theta \right)} - a \cos{\left(\theta \right)} + a \cos{\left(T w + \theta \right)} + \left(T w a + w v\right) \sin{\left(T w + \theta \right)}\right)}{w^{3}} & \frac{T w \sin{\left(T w + \theta \right)} - \cos{\left(\theta \right)} + \cos{\left(T w + \theta \right)}}{w^{2}}\\0 & 1 & \frac{- w v \sin{\left(\theta \right)} - a \cos{\left(\theta \right)} + a \cos{\left(T w + \theta \right)} - \left(- T w a - w v\right) \sin{\left(T w + \theta \right)}}{w^{2}} & \frac{w \cos{\left(\theta \right)} - w \cos{\left(T w + \theta \right)}}{w^{2}} & \frac{T a \cos{\left(T w + \theta \right)} - T \left(- T w a - w v\right) \sin{\left(T w + \theta \right)} + v \cos{\left(\theta \right)} + \left(- T a - v\right) \cos{\left(T w + \theta \right)}}{w^{2}} - \frac{2 \left(w v \cos{\left(\theta \right)} - a \sin{\left(\theta \right)} + a \sin{\left(T w + \theta \right)} + \left(- T w a - w v\right) \cos{\left(T w + \theta \right)}\right)}{w^{3}} & \frac{- T w \cos{\left(T w + \theta \right)} - \sin{\left(\theta \right)} + \sin{\left(T w + \theta \right)}}{w^{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{matrix}\right] JF=⎣⎢⎢⎢⎢⎢⎢⎡100000010000w2−wvcos(θ)+asin(θ)−asin(Tw+θ)+(Twa+wv)cos(Tw+θ)w2−wvsin(θ)−acos(θ)+acos(Tw+θ)−(−Twa−wv)sin(Tw+θ)1000w2−wsin(θ)+wsin(Tw+θ)w2wcos(θ)−wcos(Tw+θ)0100w2−Tasin(Tw+θ)+T(Twa+wv)cos(Tw+θ)−vsin(θ)+(Ta+v)sin(Tw+θ)−w32(−wvsin(θ)−acos(θ)+acos(Tw+θ)+(Twa+wv)sin(Tw+θ))w2Tacos(Tw+θ)−T(−Twa−wv)sin(Tw+θ)+vcos(θ)+(−Ta−v)cos(Tw+θ)−w32(wvcos(θ)−asin(θ)+asin(Tw+θ)+(−Twa−wv)cos(Tw+θ))T010w2Twsin(Tw+θ)−cos(θ)+cos(Tw+θ)w2−Twcos(Tw+θ)−sin(θ)+sin(Tw+θ)0T01⎦⎥⎥⎥⎥⎥⎥⎤
这也太长了太复杂了，谁会用这玩意儿。尽管求导一次就行，之后代数进去就行，但是也超级麻烦呀。
4 关于Q矩阵的公式推导
关于Q矩阵，一般是利用 Δ t \Delta t Δt和模型的噪声误差来赋值。(我以前是直接手工粗暴调的，因为不同模型的噪声方差基本上也都是自己设定的，差别不大。但是尽量按公式来吧。)
4.1 CTRV模型
比如对于CTRV，假设的直线加速度 a a a_a aa和偏航角加速度 a w a_w aw为常数，但是实际过程中会有噪声，因此CTRV的模型噪声主要来自 a a a_a aa和 a w a_w aw。假设该模型噪声都符合高斯分布，因此 a a ∼ N ( 0 , σ a 2 ) a_a \sim N(0,\sigma_a^2) aa∼N(0,σa2)， a w ∼ N ( 0 , σ w 2 ) a_w \sim N(0,\sigma_w^2) aw∼N(0,σw2)。
a a , a w a_a,a_w aa,aw这两个加速度量对状态量( x , y , v , θ , ω x,y,v,\theta,\omega x,y,v,θ,ω)的预测噪声为：
w = [ 1 2 Δ t 2 c o s ( θ ) a a 1 2 Δ t 2 s i n ( θ ) a a Δ t a a 1 2 Δ t 2 a ω Δ t a ω ] = [ 1 2 Δ t 2 c o s ( θ ) 0 1 2 Δ t 2 s i n ( θ ) 0 Δ t 0 0 1 2 Δ t 2 0 Δ t a ω ] [ a a a w ] = G u w = \begin{bmatrix} \frac 12 \Delta t^2 cos(\theta) a_a \\ \frac 12 \Delta t^2 sin(\theta) a_a \\ \Delta t a_a \\ \frac 12 \Delta t^2 a_{\omega} \\ \Delta t a_{\omega} \end{bmatrix} = \begin{bmatrix} \frac 12 \Delta t^2 cos(\theta) & 0 \\ \frac 12 \Delta t^2 sin(\theta) & 0 \\ \Delta t & 0 \\ 0 & \frac 12 \Delta t^2 \\ 0 & \Delta t a_{\omega} \end{bmatrix} \begin{bmatrix} a_a \\ a_w \end{bmatrix} = Gu w=⎣⎢⎢⎢⎢⎡21Δt2cos(θ)aa21Δt2sin(θ)aaΔtaa21Δt2aωΔtaω⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡21Δt2cos(θ)21Δt2sin(θ)Δt0000021Δt2Δtaω⎦⎥⎥⎥⎥⎤[aaaw]=Gu
Q = c o v ( w ) = E ( w w T ) = G E ( u u T ) G T = G [ σ a 2 0 0 σ w 2 ] G T Q = cov(w) = E(w w^T) = GE(uu^T)G^T = G \begin{bmatrix} \sigma_a^2 & 0 \\ 0 & \sigma_w^2 \end{bmatrix} G^T Q=cov(w)=E(wwT)=GE(uuT)GT=G[σa200σw2]GT
Q = [ 0.25 T 4 σ a 2 cos 2 ( θ ) 0.25 T 4 σ a 2 sin ( θ ) cos ( θ ) 0.5 T 3 σ a 2 cos ( θ ) 0 0 0.25 T 4 σ a 2 sin ( θ ) cos ( θ ) 0.25 T 4 σ a 2 sin 2 ( θ ) 0.5 T 3 σ a 2 sin ( θ ) 0 0 0.5 T 3 σ a 2 cos ( θ ) 0.5 T 3 σ a 2 sin ( θ ) T 2 σ a 2 0 0 0 0 0 0.25 T 4 σ w 2 0.5 T 3 σ w 2 0 0 0 0.5 T 3 σ w 2 T 2 σ w 2 ] Q = \left[\begin{matrix}0.25 T^{4} \sigma_a^{2} \cos^{2}{\left (\theta \right )} & 0.25 T^{4} \sigma_a^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} & 0.5 T^{3} \sigma_a^{2} \cos{\left (\theta \right )} & 0 & 0\\0.25 T^{4} \sigma_a^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} & 0.25 T^{4} \sigma_a^{2} \sin^{2}{\left (\theta \right )} & 0.5 T^{3} \sigma_a^{2} \sin{\left (\theta \right )} & 0 & 0\\0.5 T^{3} \sigma_a^{2} \cos{\left (\theta \right )} & 0.5 T^{3} \sigma_a^{2} \sin{\left (\theta \right )} & T^{2} \sigma_a^{2} & 0 & 0\\0 & 0 & 0 & 0.25 T^{4} \sigma_w^{2} & 0.5 T^{3} \sigma_w^{2}\\0 & 0 & 0 & 0.5 T^{3} \sigma_w^{2} & T^{2} \sigma_w^{2}\end{matrix}\right] Q=⎣⎢⎢⎢⎢⎡0.25T4σa2cos2(θ)0.25T4σa2sin(θ)cos(θ)0.5T3σa2cos(θ)000.25T4σa2sin(θ)cos(θ)0.25T4σa2sin2(θ)0.5T3σa2sin(θ)000.5T3σa2cos(θ)0.5T3σa2sin(θ)T2σa2000000.25T4σw20.5T3σw20000.5T3σw2T2σw2⎦⎥⎥⎥⎥⎤
4.2 CV模型
对于CV模型的话，噪声就是直线加速度a，其分量 a x a_x ax和 a y a_y ay对状态量( x , y , v x , v y x,y,v_x,v_y x,y,vx,vy)预测噪声为：
w = [ 1 2 Δ t 2 a x 1 2 Δ t 2 a y Δ t a x Δ t a y ] = [ 1 2 Δ t 2 0 0 1 2 Δ t 2 Δ t 0 0 Δ t ] [ a x a y ] = G u w = \begin{bmatrix} \frac 12 \Delta t^2 a_x \\ \frac 12 \Delta t^2 a_y \\ \Delta t a_x \\ \Delta t a_y \end{bmatrix} = \begin{bmatrix} \frac 12 \Delta t^2 & 0 \\ 0 & \frac 12 \Delta t^2 \\ \Delta t & 0 \\ 0 & \Delta t \end{bmatrix} \begin{bmatrix} a_x \\ a_y \end{bmatrix} = Gu w=⎣⎢⎢⎡21Δt2ax21Δt2ayΔtaxΔtay⎦⎥⎥⎤=⎣⎢⎢⎡21Δt20Δt0021Δt20Δt⎦⎥⎥⎤[axay]=Gu
Q = c o v ( w ) = E ( w w T ) = G E ( u u T ) G T = G [ σ x 2 0 0 σ y 2 ] G T = [ 0.25 T 4 σ x 2 0 0.5 T 3 σ x 2 0 0 0.25 T 4 σ y 2 0 0.5 T 3 σ y 2 0.5 T 3 σ x 2 0 T 2 σ x 2 0 0 0.5 T 3 σ y 2 0 T 2 σ y 2 ] Q = cov(w) = E(w w^T) = GE(uu^T)G^T = G \begin{bmatrix} \sigma_x^2 & 0 \\ 0 & \sigma_y^2 \end{bmatrix} G^T = \left[\begin{matrix}0.25 T^{4} \sigma_x^{2} & 0 & 0.5 T^{3} \sigma_x^{2} & 0\\0 & 0.25 T^{4} \sigma_y^{2} & 0 & 0.5 T^{3} \sigma_y^{2}\\0.5 T^{3} \sigma_x^{2} & 0 & T^{2} \sigma_x^{2} & 0\\0 & 0.5 T^{3} \sigma_y^{2} & 0 & T^{2} \sigma_y^{2}\end{matrix}\right] Q=cov(w)=E(wwT)=GE(uuT)GT=G[σx200σy2]GT=⎣⎢⎢⎡0.25T4σx200.5T3σx2000.25T4σy200.5T3σy20.5T3σx20T2σx2000.5T3σy20T2σy2⎦⎥⎥⎤