無損卡爾曼濾波UKF（1）-從CTRV過程模型開始
無損卡爾曼濾波
- 處理非線性過程模型和非線性測量模型的替代方法
- 不會對非線性函數進行線性化處理
- 用所謂的sigma點來近似概率分布
- 省去計算雅閣比矩陣的部分
過程模型
1 CV模型的缺陷：
In the extended kalman filter lesson, we used a constant velocity model (CV).
A constant velocity model is one of the most basic motion models used with object tracking.
- 實際上簡化了車輛的運動方式，大多數道路是有轉彎的。
- 速度為常量，拐彎的你就沒法整了。
2 其他模型
- constant turn rate and velocity magnitude model (CTRV)
- constant turn rate and acceleration (CTRA)
- constant steering angle and velocity (CSAV)
- constant curvature and acceleration (CCA)
3 CTRV 模型確定性部分推導
- 固定的轉彎速率
- 恆定的速度大小
3.1 狀態向量
\(x = [px,py,v,\phi,\dot{\phi}]^T\)
- 位置px py
- 速度大小 v
- 偏航角 φ
- 角速度 φ.
3.2 過程模型推導
\(x_{k+1} = f(x_k,v_k);\)
噪聲向量vk就是需要我們建模時推導的函數
先討論狀態x的變更速率
\(x = [px,py,v,\phi,\dot{\phi}]^T\)
\(\dot{x} = [\dot{p_x},\dot{p_y},\dot{v},\dot{\phi},\ddot{\phi}]^T\)
其中g(x)的各變量
\(\dot{p_x} = v_x = cos(\phi) \cdot v\)
\(\dot{p_y} = v_y = sin(\phi) \cdot v\)
\(\dot{v} = 0\)
\(\dot{\phi} = \dot{\phi}\)
\(\ddot{\phi} = 0\)
推出差分方程
\(\dot{x} = g(x) = [cos(\phi)\cdot v, sin(\phi)\cdot v , 0 ,\dot{\phi} ,0 ]^T\)
求時間差
\(\Delta t = t_{k+1} - t_k\)
在時間差內求積分 就可以求得時間差內各項的變化，也就是確定性部分的預測方程
\(x_{k+1} = x_k + \int^{t_{k+1}}_{t_k}{g(x)}{\rm d}t\)
特殊情況：角速度為0的時候
\(p_{x_{k+1}} = p_{x_k} + cos(\phi)\cdot v_k \cdot \Delta t\)
\(p_{y_{k+1}} = p_{y_k} + sin(\phi)\cdot v_k \cdot \Delta t\)
4 CTRV 模型噪聲向量推導
過程模型噪聲分為兩個部分
我們的CTRV模型，假設了車的縱向速度和角速度為恆定值，忽略了速度的變化。
因此要把加速度項的影響放到誤差項里面。
1. 縱向加速度噪聲va，表示在縱向速度上的不確定性，因此可以定義為，正態分布的白噪聲，以0為均值
\(\nu_{a,k} - N (0,\sigma^2_a )\)
2. 角加速度噪聲vφ..，表示偏航角速度上的不確定性，因此可以定義為，正態分布的白噪聲，以0為均值
\(\nu_{\ddot{\phi},k} - N (0,\sigma^2_{\ddot{\phi}} )\)
因此，過程噪聲向量為：
\(\nu_k = [ \nu_{a,k} , \nu_{\ddot{\phi},k} ]^T\)
考慮噪聲向量對整個過程的影響：
假設誤差項-偏航角角加速度和縱向加速度是常量，那么在時間差內產生的誤差a,b,c,d,e ，就可以定義為
\(a = (1/2) \cdot \nu_{a,k} \cdot cos(\phi_k) \cdot (\Delta t )^2\)
\(b = (1/2) \cdot \nu_{a,k} \cdot sin(\phi_k) \cdot (\Delta t )^2\)
\(c = \Delta t \cdot \nu_{a,k}\)
\(d = (1/2)\cdot \nu_{\ddot{\phi},k} \cdot (\Delta t )^2\)
\(e = \Delta t \cdot \nu_{\ddot{\phi},k}\)
注意上式中的
\(\nu_{a,k}\)- 和 -\(\nu_{\ddot{\phi},k}\)
分別代表誤差項中的縱向加速度項和角加速度項，並不是速度。