5分鐘梳理移動機器人定位實驗知識點

定位與導航和一道哲學問題很相像，我們是誰？我們從何出來？我們要到何處去？

那，玩遊戲前，明確我們的身份：我們是一個簡易的移動小車，當我們處於客觀的世界時，我們從世界座標系下的（0,0,0）來，我們想要到達$^*P(^*x,^*y,^*\theta)$去，中間的每一個狀態是$(x,y,\theta)$。當我們專注於自我世界時，我們停留在主觀世界，我們衡量任何東西都是在自身的座標系下$Frame M(x_m,y_m,\theta_m)$。分別描述成longitudinal,lateral,heading。
易錯點error = final position – calculated position

【考點1】Classes of location

【考點2】Discrete Evolution Equation
$\left\{\begin{array}{l} X_{k+1}=f\left(X_{k}, U_{k}\right) \\ U_{k}=\left[\Delta D_{k}, \Delta \theta_{k}\right]^{T} \end{array}\right.$

【考點3】卡爾曼濾波器實驗
複習卡爾曼濾波器在移動機器人定位中的應用（上）之後開始整合實驗內容。
我們希望控制的是小車的速度和角速度$U=[\Delta D_k,\Delta \theta_k]$，但實際上我們控制的是小車左右輪子的轉速$\Delta q = [\Delta q_r,\Delta q_l]^T$。

兩輪差速運動模型推導(補充)

$v_l=r_l \dot q_l,v_r = r_r \dot q_r$
$d_{sl} = lim_{\Delta t \rightarrow 0} \dot q_l r_l \Delta t$
$d_{sr} = lim_{\Delta t \rightarrow 0} \dot q_r r_r \Delta t$
$d\theta = lim_{\Delta t \rightarrow 0} \frac{d_{sr}-d_{sl}}{L}$
$v = \frac{r_r\dot q_r + r_l\dot q_l}{2}$
$\omega = \frac{d\theta}{dt} = \frac{r_r \dot q_r - r_l \dot q_l}{L}$

【考點3.1】卡爾曼濾波器實驗協方差矩陣

1. 測量誤差$Q_\gamma$
   $Q_{\gamma}=\operatorname{diag}\left(\sigma_{mx}^{2}, \sigma_{my}^{2}\right)$易錯點，這個不是高斯分佈是均勻分佈。因爲小車在平面上漫遊點亮8個彈簧開關的概率是一樣的。
   $\sigma_{mx}^{2} = \frac{(b-a)^2}{12}=\frac{(10-(-10))^2}{12}$
   $\sigma_{my}^{2} = \frac{(b-a)^2}{12}=\frac{(5-(-5))^2}{12}$
2. 輸入誤差$Q_\beta(2\times2)$
   $U = K \Delta q$,其中K是兩者之間的線性關係。
   我們可以得到協方差之間的關係。
   $Q_\beta = KQ_{\dot q}K^T$

By linearizing an input-output relation, it is possible to calculate the covariance matrix of the errors on output variables as a function of the covariance matrix of input variables.

在這裏，我們把所有輸入性質的誤差都簡化到兩個輪子這裏，這裏的誤差來源不僅僅來源於距離測量編碼器還有可能是輪子與路面不是點接觸，兩個輪子旋轉軸不重合（換一句話說，兩個輪子不垂直）等等。我們不可能單純地用編碼器來算出$Q_\beta$,$Q_\beta$是需要我們在實驗中調試出來。

3. 位置誤差?不確定$Q_\alpha$
   在我們的實驗裏面並沒有對角度$\theta$的測量。

   $Q_{\alpha}=\operatorname{diag}\left(\sigma^{2}, \sigma^{2}, \sigma_{\theta}^{2}\right)$

4. Mahalanobis distance threshold

mahalanobis距離是基於樣本分佈的一種距離。物理意義就是在規範化的主成分空間中的歐氏距離。所謂規範化的主成分空間就是利用主成分分析對一些數據進行主成分分解。再對所有主成分分解軸做歸一化，形成新的座標軸。由這些座標軸張成的空間就是規範化的主成分空間。
換句話說，主成分分析就是把橢球分佈的樣本改變到另一個空間裏，使其成爲球狀分佈。而mahalanobis距離就是在樣本呈球狀分佈的空間裏面所求得的Euclidean距離。

易錯點這個距離是沒有量綱的。

GPS (X,Y) measurement：Which cell of of the table do you use to define Mahalanobis distance threshold? (Y = (x,y,theta) )

卡方檢驗是統計樣本的實際觀測值與理論推斷值之間的偏離程度，實際觀測值與理論推斷值之間的偏離程度就決定卡方值的大小，如果卡方值越大，二者偏差程度越大；反之，二者偏差越小；若兩個值完全相等時，卡方值就爲0，表明理論值完全符合。
這裏我們需要學習看卡方分佈表，兩個參數一個是$n$（dimation of Y），另外一個參數是$\alpha$（ $1-\alpha = 0.95$）。
α代表分位點，$Χ^2$ 1-α (n)中，表示當自由度爲n時，卡方分佈$Χ^2$(n)取值大於該點$Χ^2$ 1-α (n)的概率爲α。
我們實驗中用的是Matlab裏面的chi2inv(0.95,2)函數。

【考點3.2】卡爾曼濾波器實驗A，B，C矩陣

function Xnew = EvolutionModel( Xold , U )

% Recall X=(x,y,theta)' and U=(deltaD,deltaTheta)'

Xnew = Xold + [ U(1)*cos(Xold(3)) ;
                U(1)*sin(Xold(3)) ;
                U(2)              ;
              ] ;
          
return  

deter all the size of the matrix(Jacobian)

求解出方程前，就可以確定A，B，C矩陣的大小(size)。
$A_{k}=\frac{\partial f}{\partial X}\left(\widehat{X}_{k / k}, U_{k}^{*}\right) \quad \text { and } \quad B_{k}=\frac{\partial f}{\partial U}\left(\widehat{X}_{k / k}, U_{k}^{*}\right)$
$C_{k}=\frac{\partial g}{\partial X}\left(\widehat{X}_{k+1 / k}\right)$

$f(3\times1),X(3\times1)$,那麼A$(3\times3)$
$f(3\times1),U(2\times1)$,那麼B$(3\times2)$
$g(2\times1),X(3\times1)$,那麼C$(2\times3)$

古代地中海地區的水手們通過觀察天體運行的方式來爲船隻提供導航的手段，在白天，水手們通過觀察太陽不同季節在天空中運行的軌跡來判斷方位。在夜間，水手們則依靠觀察星座的位置來進行導航，但是，對於地中海地區的古希臘人來說，使用星座進行導航的方式，出現在其文明發展的中後期，這一時期的古希臘人在天文學上有了相當的積累，他們可以辨識天空的星座並且能描繪出某些星星的運行軌跡，然而在古希臘文明的早期，水手們是無法使用星座進行導航的。 ——歷史野談

從不辨東西到星辰大海——人類導航技術史初探

今日背景音樂淮上的《破雲》官方主題曲

如心中有答案
未被認同
我繼續追尋
並非故作英勇
都忘了就輕鬆
但如何肯放縱
能與你破雲揭霧
這光景何等光榮

餘暉下的浮塵
七月未央的黃昏
回憶裏盤旋上升
誰的眼神
沿着逆光捕捉側身
感受到一瞬間溫存 (鑽石出現裂痕)

千瘡百孔的疼
從不會使我消沉
更容易看清靈魂
好人壞人
何必在乎我真實身份
這故事沒有劇本
只有熱忱

如面具有代價
願承其重
我視死如歸
當然由始至終
這世上的種種
有幾人看得懂
即使命運是牢籠
也不屑被它附庸