機器人-數學工具

1. 基本概念

• LIDAR（LIght Detection And Ranging）：光探測和測距
• LADAR（LAser Detection And Ranging）：激光探測和測距
• $\color{red}{均值、方差、標準差}$：用於描述一維隨機變量 (即一維隨機變量$X=(x)$，多個樣本)
• $\color{red}{ 協方差、相關係數、協方差矩陣}$：用於描述多維隨機變量（即多維隨機變量$X=(x, y,z)$, 多個樣本）
• $\color{red}{協方差矩陣}$：它計算的是不同維度之間的協方差，而不是不同樣本之間
• 協方差矩陣定義了我們數據的傳播（方差）和方向（協方差）
• 協方差<->特徵向量<->雅可比矩陣

1.1 概率基本概念

1.1.1 隨機變量

• 不確定性（Uncertainty）：不確定性在現實世界中是客觀存在的；在現實世界中，不確定性來自於片面的、間接的和模糊的觀察，觀測值（e.g. sensor noise），動作執行（有限制的控制系統）
• 概率論（Probability Theory）：$\color{red}{是處理不確定性問題最強有力的方式}$
• 隨機變量（Random Variable）：用於表示一個不確定性的量，記作：$x$
  • 連續隨機變量（Continuous）：其值爲實數值（有限或無限的），其概率分佈$f(x)$叫做概率密度函數（pdf: Probability Density Function）。其特性爲：$p(x) \ge 0 \quad \int f(x)\,dx = 1$
    
  • 離散隨機變量（Discrete）：其值爲預定義的集合(有序、無序、有限或無限的)，其概率分佈$p(x)$叫做概率質量函數（Probability Mass Function）,經常以直方圖或Hinton圖表示，其特性爲：
    $p(x) \ge 0 \quad \sum p(x) = 1$
    
  • 正態分佈概率密度函數
    $X \sim N(\mu, \sigma^2) \quad f(x) = \frac {1}{\sqrt {2 \pi}\sigma} e ^{-\frac{(x-\mu)^2}{2\sigma^2}}$

1.1.2 隨機向量

• 隨機向量（Random Vector）：包含多個隨機變量的向量爲隨機向量

• 正態分佈：
  $X = [x_1, x_2, ..., x_k]^T ：k個變量, \quad X \sim N_k(\mu, \Sigma)$

• 均值向量：
  $\mu = E[X] = [E[x_1], E[x_2], ..., E[x_k]]^T$

• kxk階協方差矩陣:
  $\Sigma =:E[(X-\mu)(X-\mu)^T] = [Cov[X_i,Y_j]; 1 \leq i,j \leq k]$

• 概率密度函數：
  $f_X(x_1, ..., x_k) = \frac{1}{\sqrt{(2\pi)^k|\Sigma|}}e^{-\frac{1}{2}(X-\mu)^T\Sigma^{-1}(X-\mu)}$

1.1.3 概率

• 概率（Probability）：指隨機變量取某一個值的機率，記作：$p(x)$

• 聯合概率（Joint Probability）：指兩個或多個事件同時發生的概率，記作：$p(x,y,z)$，其隨機變量可能全部是離散的，或全部是連續的，或者是混合的；其總和或積分值一定是1。其中$p(x,y,z)可以記作p(X), X=[x, y, z]^T$

• 邊緣概率（Marginal Probability）：指一個事件$x$發生的概率，記作：$p(x)$，根據 多個隨機變量的聯合概率求部分隨機變量的概率的過程（求和或積分），被稱爲邊緣化（marginalization）
  $f(x) = \int f(x,y) \, dy \quad p(y) = \int f(x,y) \, dx \qquad(x,y是連續的)$
  $p(x,y)=\sum_w \int f(x,y,z,w) \, dz \qquad (x,y是離散的，z是連續的，w是離散的)$

• 條件概率（Conditional Probability）：對於聯合概率$p(x,y）$，當$y=y^*$時$x$的概率，記作：$p(x|y=y^*)$，簡記爲$p(x|y)$。$p(x,y)$的積分或求和的結果爲1，而$p(x|y=y^*)$只是其中的一部分，所以其積分或和小於1，爲滿足其積分或和的結果爲1，需要對其進行歸一化，其計算公式如下：
  $p(x|y=y^*)=\frac {p(x,y=y^*)}{p(y=y^*)}=\frac{p(x,y=y^*)}{\int p(x,y=y*)dx}=p(x|y)=\frac{p(x,y)}{p(y)}$

• 三者的關係：
  

• 概率和爲1:

  • 離散型
    $\sum_x p(x) =1$
  • 連續型
    $\int p(x)dx = 1$

• 總概率定律:

  • 離散型
    $p(x) = \sum_yp(x|y)p(y)$
  • 連續型
    $p(x) = \int p(x|y)p(y)dy$

• 邊緣概率:

  • 離散型
    $p(x) = \sum_y p(x,y)$
  • 連續型
    $p(x) = \int p(x,y)dy$

• Bayes公式：
  $p(x,y) = p(x|y)p(y) = p(y|x)p(x)$

• 條件獨立
  $p(x,y|z) = p(x|z) p(y|z)$
  $p(x,y) = p(x)p(y)$

1.2 貝葉斯定理

• 貝葉斯定理（Bayes’ Rule ）：
  • 允許我們利用已有的知識或者信念（belief）（通常是先驗知識prior）幫助我們計算相關事件的概率。
    
    $p(x,y) = p(x|y)p(y) = p(y|x)p(x)$
    $P(x|y)= \frac{p(y|x)p(x)}{p(y)} = \frac {likelihood \cdot prior }{ evidence} = posterior$
  • $p(x)$：可以是一個值（先驗值），也可以是一個分佈（先驗分佈）
  • $p(y|x)$：似然分佈，也叫證據 (y就是新的數據 證據)
  • $p(x|y)$：後驗分佈，由新的數據（證據）更新之後的分佈
  • 分母$p(y)$：是一個歸一化常量，用於確保所有的後驗概率之和爲1
    $p(x|y) = \eta \cdot p(y|x) \cdot p(x) = \frac {p(y|x) \cdot p(x)}{p(y)}$
  • 給定背景知識e,Bayes規則變爲：
    $p(x|y,e) = \frac{p(y|x,e)\cdot p(x|e)}{p(y|e)}$
  • 學習能力：Bayes Rule允許把新的數據證據與已有經驗組合在一起，且用新的數據證據更新已經有的經驗，即具有學習功能。
• 鏈式規則（Chain Rule）：
  $p(x_1,x_2,...,x_K)=\prod_{i=1}^K \, p(x_i | x_1,...,x_{i-1})$

1.2.1 基於多個證據$(z_1,..., z_n)$的狀態估計(遞歸Bayes更新)

$p(x|z_1,...,z_n) = \frac{p(z_n|x,z_1,...,z_{n-1})p(x|z_1,...,z_{n-1})}{p(z_n|z_1,...,z_{n-1})}$

1.2.2 Markov假設

• $z_n獨立於z_1,...,z_{n-1}$, 則有：
  $p(x|z_1,...,z_n) = \frac {p(z_n|x)p(x|z_1,...,z_{n-1})}{p(z_n|z_1,...,z_{n-1})}$
  $＝\eta p(z_n|x)p(x|z_1,...,z_{n-1}) = [\prod_{i=1...n} (\eta_i \cdot p(z_i|x))]p(x)$

1.2.3 基於動作執行的狀態估計

• 執行動作u,把狀態x’變爲x：
  $p(x|u,x&#x27;)$
• 集成動作的結果
  • 連續：
    $p(x|u) = \int p(x|u,x&#x27;)p(x&#x27;)dx&#x27;$
  • 離散：
    $p(x|u) = \sum p(x|u,x&#x27;)p(x&#x27;)$

1.2.4 Bayes Filter框架

• 給定條件：
  • 一組觀測值z和控制數據u：$d_t = \{u_1, z_1, ..., u_t, z_t\}$
  • Sensor模型：p(z|x)
  • 動作模型：p(x|u,x’)
  • 系統狀態的先驗概率(Prior)：p(x)
  • z：觀測
  • u：動作
  • x：狀態
• 目標：
  • 估計動態系統的狀態x
  • 狀態的後驗概率也叫做Belief：$Bel(x_t) = p(x_t|u_1, z_1, ..., u_t, z_t)$
• Markov假設
  

1.2.5 Bayes Filters

$Bel(x_t) = p(x_t | u_1, z_1, ..., u_t, z_t)$
$\color{red}{Bayes}$ $\quad=\eta \, p(z_t|x_t,u_1,z_t,...,u_t) p(x_t | u_1,z_1,...,u_t)$
$\color{red}{Markov}$ $\quad=\eta \, p(z_t|x_t)p(x_t|u_1,z_1,...,u_t)$
$\color{red}{Total Prob.}$ $=\eta \, p(z_t|x_t) \int p(x_t|u_1,z_1,...,u_t,x_{t-1}) p(x_{t-1}|u_1,z_1,...,u_t) dx_{t-1}$
$\color{red}{Markov}$ $=\eta \, p(z_t|x_t) \int p(x_t|u_t,x_{t-1}) p(x_{t-1}|u_1,z_1,...,u_t) dx_{t-1}$
$\color{red}{Markov}$ $=\eta \, p(z_t|x_t) \int p(x_t|u_t,x_{t-1}) p(x_{t-1}|u_1,z_1,...,u_{t-1}, z_{t-1}) dx_{t-1}$
$\quad\quad =\eta \, p(z_t|x_t) \int p(x_t|u_t,x_{t-1}) Bel(x_{t-1})dx_{t-1}$

1.3 概率圖模型

• 概率圖模型（Probabilistic Graphical Models）
  • 有向圖模型用於表示隨機變量的聯合分佈
    
• 馬爾可夫模型（Markov Models）
  • 模型假設對未來的預測僅僅依賴於最後一個觀測值
    
    $p(x_1, x_2, ..., x_K) = p(x_1) \prod_{i=2}^K p(x_i | x_{i-1})$
• 齊次馬爾可夫模型(Homogeneous Markov Model)：所有$p(x_i|x_{i-1})$是相同的

1.4 狀態空間模型

• 狀態空間模型（State Space Model）
  
  • $x_i$：潛在或隱藏變量（Latent or Hidden Variables），形成馬爾可夫鏈 (狀態)
  • $z_i$：觀測變量（Observations Variables）(觀測值)
  • 聯合概率分佈
    $p(x_1,...,x_K,z_1,...,z_K)=p(x_1) \{ \prod_{i=2}^K p(x_i|x_{i-1}) \} \, \prod_{i=1}^K p(z_i | x_i)$
  • 隱馬爾可夫模型(HMM: Hidden Markov Model): 隱藏變量$x_i$是離散的， 觀測變量$z_i$是離散或是連續的
  • 線性動態系統（LDS: Linear Dynamical System）：隱藏變量$x_i$和 觀測變量$z_i$都是連續的，且滿足高斯分佈
  • 狀態空間模型的三個組件：
    • 變換模型(Transition Model)：描述系統是如何演變的，它定義了概率分佈$p(x_i|x_{i-1})$
    • 觀測模型（Observation/Sensor Model）：定義了概率分佈$p(z_i|x_i)$
    • 先驗概率分佈（Prior Probability Distribution）：組合所有隨機變量的聯合分佈$p(x_0)$
      
  • 狀態空間模型可完成的推理任務：
    • 濾波（Filtering）：基於所有觀測值計算後驗概率（posterior distribution）$p(x_k | z_{1:k})$；以在線方式跟蹤系統的當前狀態
    • 預測（Prediction）：計算後驗概率 $p(x_{k+t}|z_{1:k}) \qquad t&gt;0$
    • 平滑（Smoothing）：計算後驗概率 $p(x_{t}|z_{1:k}) \qquad 0 \le t \lt k$
    • 尋找最優狀態序列（Most likely sequence）：給定一個觀測序列，找到一個最有可能產生此觀測序列的狀態序列,如語音識別
      \begin{equation}
      \mathop{\arg\max}{x{1:k}} \ \mathrm{p}(x_{1:k} | z_{1:k})
      \end{equation}

1.5 線性動態系統(LDS)

• 線性高斯狀態空間模型：Linear-Gaussian State Space Model
  • $F_k, H_K$：都是線性函數
    
• 線性高斯的特點（Linear-Gaussian）
  • 線性變換不變性：高斯分佈經過線性變換之後仍然是高斯分佈
    
  • 邊緣分佈不變性：給定高斯聯合分佈，所有推導出來的邊緣分佈仍然是高斯分佈
    
  • 條件分佈不變性：給定高斯聯合分佈，所有推導出來的條件分佈仍然是高斯分佈
    
  • 乘積分佈不變性：兩個高斯分佈相乘仍然是高斯分佈
    
• LDS定義：基於線性高斯假設的時間模型（Temporal Model）
• LDS表示：
  
  
  • LDS假設：所有噪聲隨機變量相互獨立。
  • $F、G$：根據運動學進行計算（即根據運動學列出每一個狀態變量的方程，然後求得$F$和$G$）
  • $H$：根據觀測值與狀態值間的關係列出方程，然後求出觀測矩陣$H$

1.6 常用概率分佈

1.6.1 高斯分佈/正態分佈（Gaussian Distribution）

• 連續隨機變量最廣泛使用的分佈
• 特徵：
  • 簡單：僅由兩個矩（參數）表示，均值(mean)和方差(variance)
  • 中心極限定理：Central Limit Theorem (CLT)
• ** 一元概率密度函數（PDF）**
  $\mathcal{N_x}(\mu, \sigma^2)=p(x) = \frac {1}{\sigma \sqrt {2 \pi}} e ^{-\frac {(x-\mu)^2}{2 \sigma^2}}$
  
• 多元概率密度函數（PDF）
  • $x$爲D維隨機向量$\{x_1,x_2, ..., x_D \}$
    $\mathcal{N_x}(\mu, \Sigma) = p(x) = \frac {1}{|\Sigma|^{1/2}\,(2 \pi)^{D/2}} exp \{ - \frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu) \}$
    

2. 參數估計方法

• 參數估計的目的：根據觀測數據，估計其概率分佈的參數，使觀測數據出現的概率最大。
• （統計學）推斷：是從數據中推導一個概率分佈性質的過程。
• 參數估計的步驟：
  • 根據觀測值，確定數據分佈的模型
  • 用有效的方法求模型的參數
  • 參數：在機器學習中，通常用一個模型來描述我們觀測到的數據，每個模型都有一組自己的參數，用於決定模型的質量，如高斯（正態）分佈的參數爲$(\mu, \sigma)$
• 數據濾波：是$\color{red}{去除噪聲還原真實數據}$的一種數據處理技術
• 狀態量：受噪聲干擾的狀態量是個隨機量，不可能測得精確值，但可對它進行一系列觀測，並依據一組觀測值，按某種統計觀點對它進行估計。使估計值儘可能準確地接近真實值，這就是最優估計。真實值與估計值之差稱爲估計誤差。若估計值的數學期望與真實值相等，這種估計稱爲無偏估計。
• 狀態估計：一般來說，根據觀測數據對隨機量進行定量推斷就是估計問題，特別是對動態行爲的狀態估計，它能實現實時運行狀態的估計和預測功能。比如對飛行器狀態估計。狀態估計對於瞭解和控制一個系統具有重要意義，所應用的方法屬於統計學中的估計理論。
  • 最常用的估計方法：
    • 最小二乘估計
    • 線性最小方差估計
    • 最小方差估計
    • 遞推最小二乘估計
  • 基於風險準則的估計方法：
    • 貝葉斯估計
    • 最大似然估計
    • 隨機逼近
• 高斯白噪聲（White Gaussian Noise）：即噪聲與時間和空間是沒有關係的，而且符合相同的高斯分佈（Gaussian Distribution）

2.1 極大似然估計（MLE: Maximum Likelihood Estimation）

• MLE用途：是估計模型參數的一種方法，通過最大化觀測數據的總概率得到最終的參數值。
• MLE原理：$\color{red}{因爲已經觀測到這些樣本數據，所以這些樣本數據的概率應該極大（最大）}$，即最大化觀測數據的總概率。（即給定一系列觀測數據點，我們進行極大似然估計得到參數的估計，相當於得到關於數據集中這個參數變量的均值信息）
• MLE求解步驟：
  • 似然函數（Likelihood function）：給定觀測數據，模型參數取特定值的可能性
  • 寫出似然函數：$L(\theta) = L(x_1,x_2,…,x_n; \theta_1, \theta_1, …, \theta_m),x_i爲觀測數據，\theta_j爲模型參數 $；觀測數據的似然函數$L(\theta)$是$\theta$的函數，隨$\theta$的變化而變化
  • 對似然函數取自然對數：即對數似然（Log Likelihood），用於去指數，把乘法變成加法，把除法變成減法，以方便後面求導 ；因爲$ln(x)$是單調函數，所以$ln(x)$與$L(\theta)$的極值點位於相同的位置）
  • 對各參數求偏導，令導函數爲0，組成似然方程組
  • 解似然方程組，求得各個模型參數($\color{red}{此模型使得觀測到這些樣本數據的概率最大}$)
• 離散型似然函數
  • 若X爲離散型， 其概率分佈爲：$P(X=x)=p(x;\theta)$ (分號後面的爲參數)
  • $X$取到觀測值$x_1, x_2, ..., x_n$的概率爲：$L(\theta)=L(\theta; x_1,...,x_n)=\prod_{i=1}^n p(x_i;\theta)$
• 連續型似然函數
  • 若X爲連續型， 其概率密度函數爲：$f(x;\theta)$ (分號後面的爲參數)
  • $X$取到觀測值$x_1, x_2, ..., x_n$的概率爲：$L(\theta)=L(\theta; x_1,...,x_n)=\prod_{i=1}^n f(x_i;\theta)$

2.1.1 參數的極大似然與數據的極大概率

• 極大似然(Maximum Likelihood) ：$L(\theta; data)$，給定觀測數據，得到$\color{red}{參數的似然}$
• 極大概率(Maximum Probability)：$p(data; \theta)$，給定參數，得到$\color{red}{觀測數據的概率}$
• 二者關係
  $L(\theta; data) = p(data; \theta)$

2.1.2 極大似然估計與最小二乘法

• 最小二乘法（Least Squares）: 在機器學習中，常用於估計模型參數的方法
• 若模型是高斯的，則極大似然估計與最小二乘法等價，證明
• 可以通過理解兩個方法的目標來解釋他們。對最小二乘估計，我們想要找到一條線，來最小化全部數據點和迴歸線的平方距離（可以看下面的圖）。對於極大似然估計，我們想要最大化數據的總概率。當假設爲高斯分佈時，數據點離均值近的時候得到最大概率。由於高斯分佈是對稱的，這等價於最小化數據點和均值之間的距離。

2.2 貝葉斯推斷進行參數估計(MAP估計)

• 貝葉斯推斷(BI)進行參數估計：Bayesian Inference for Parameter Estimation
• 用途：貝葉斯推斷是給定觀測數據，利用貝葉斯定理來推導概率分佈的性質。
• 原理：先驗分佈通過更新證據（evidence），從而計算出後驗分佈，然而求出後驗分佈的MAP。
• 計算公式：$\color{red}{後驗分佈＝似然分佈*先驗分佈}$
• BI與MLE的關係：BI可視爲MLE的一般化方法。
• 待估參數：我們通常用$\Theta$表示感興趣的事件，它表示一組參數；下面以估計高斯分佈的參數爲例，則$\Theta$表示高斯分佈的均值$\mu$和標準差$\theta$（公式表示爲：$\Theta = \{ \mu, \theta \}$）
• 觀測數據：以data表示，$data=\{ y_1, y_2, ..., y_n\}$
• 按貝葉斯定理，可寫成如下公式：
  $p(\Theta| data) = \frac {p(data| \Theta) \, p(\Theta)} {p(data)}$
  • $p(\Theta)$：先驗分佈，表示我們根據經驗對參數值的估計。
  • $p(\Theta| data)$：後驗分佈，表示每次用觀測數據更新先驗分佈獲得的新分佈。
  • $p(data| Θ)$：似然分佈，它與極大似然估計中的似然函數$L(\Theta; data)$類似（極大似然等於極大概率），有時也被稱爲證據（evidence）。
  • $p(data)$：並不參與概率分佈的計算，它只是一個數值。$p(data)$的值可以通過觀測數據獲得，其存在的意義在於使$p(\Theta| data)$所有可能的總和或積分爲1。

2.2.1 實例

• 三個分佈如下圖所示：
  
• 藍色分佈：先驗分佈(prior distribution)，μ=3.6，標準差σ=0.2
• 金色分佈：根據５個金色數據點產生的似然分佈(likelihood distribution)
• 粉色分佈：由先驗分佈和似然分佈相乘產生的後驗分佈(posterior distribution)
• 後驗分佈最常見的統計數據之一是mode。這通常被用作估計感興趣的參數，被稱爲最大後驗概率估計（Maximum a posterior probability estimate）或者簡單地稱爲MAP估計。

2.2.2 工作原理

• 你對某物（例如參數的值）有一個先驗belief，然後接收一些數據。你可以根據上述公式來計算後驗分佈從而更新你的belief。之後，我們得到更多的數據，後驗成爲了新的先驗。我們可以用新數據來更新新的先驗，並且再次得到新的後驗。這個循環可以無限持續，所以能不斷更新你的beliefs。
• 先驗$\rightarrow$新的數據$\rightarrow$後驗$\rightarrow$新的先驗$\rightarrow$新的數據$\rightarrow$後驗
• 卡爾曼濾波器（以及它的變體）就是一個很好的例子。它被用在很多情況下，但是可能在數據科學領域最重要的應用是自動駕駛汽車。

2.2.3 MAP估計與極大似然估計

• 當先驗分佈爲均勻分佈時(即先驗分佈爲常數,與樣本無關；直觀地說，它表示缺乏先驗知識)，MAP估計等於MLE（極大似然估計）。
• 極大似然估計(MLE)可以看作是MAP估計的一個特例（沒有先驗知識的特例）

2.3 卡爾曼濾波器（Kalman Filter）

• 定義：卡爾曼濾波器是一種最優估計器，即可以從間接、不準確和不確定的觀測數據中推斷感興趣的參數。
• 卡爾曼濾波器的分類：

類別	特徵
KF	要求是一個線性模型
EKF	是一個非線性模型，通過Taylor展開進行局部線性化，在均值附件線性化
UKF	是一個非線性模型，通過無損變換UT(Unscented Transforms)進行局部線性化，但其近似度更高

• 理論基礎：基於貝葉斯推斷進行參數估計，尋找估計參數的方法爲參數的均方誤差最小化。
• 基本假設：
  • 後驗概率分佈爲高斯分佈
  • 線性系統
  • 高斯白噪聲
• 原理：如果所有噪聲符合高斯分佈，則卡爾曼濾波最小化估計參數（estimated parameters）的均方誤差（MSE: Mean Square Error）$\sigma$ ，即最大化其確定性。
• 爲什麼叫濾波器：它從有噪聲的數據中找到最好的估計，以達到濾除噪聲的目的
  • 濾波實例
• 特性：
  • 採用狀態空間描述法
  • 線性估計
  • 算法採用遞歸形式 (新的數據可以被實時處理，以獲得新的狀態 )
  • 能處理多維和非平穩的隨機過程
• 用途：Kalman濾波在測量方差已知的情況下能夠從一系列存在測量噪聲的數據中，估計動態系統的狀態。對於解決大部分的問題，他是最優，效率最高甚至是最有用的。它廣泛應用於：
  • 機器人導航
  • 控制
  • 傳感器數據融合
  • 軍事方面的雷達系統以及導彈追蹤
  • 計算機圖像處理（如人臉識別，圖像分割，圖像邊緣檢測等等）
• 高斯隨機變量線性變換特性
  • 高斯隨機變量經過線性變換之後，仍然是高斯隨機變量
  • 線性變換之後的均值和協方差如下：
    
    
  • $A\Sigma_xA^T$：叫做誤差傳播定律（Error Propagation Law）
• 狀態和狀態協方差公式
  
  
  • $\mathbf X_k和P_k$：分別爲狀態和狀態的協方差
  • $K_k$：爲卡爾曼增益矩陣

2.3.1 KF (Kalman Filter 線性)

• $\mathbf x_k 和 \mathbf z_k$：都是線性組合，即是其它變量的線性組合
• $\mathbf x(k|k) 、P(k|k)$：$k$時刻的狀態及狀態協方差，它基於所有至$k$時刻的觀測數據 (“prior”)
• $\mathbf x(k+1|k) 、P(k+1|k)$：$k+1$時刻的狀態及狀態協方差，它基於所有至$k$時刻的觀測數據 (“prediction”)
• $\mathbf x(k+1|k+1) 、P(k+1|k+1)$：$k+1$時刻的狀態及狀態協方差，它基於所有至$k+1$時刻的觀測數據 (“posterior”)
  
  • $\nu$：表示真正的觀測值與預測的觀測值之差
  • $H_k$：經常用於表示一個簡單的座標系變換，如預測值$\mathbf x_k$是odom座標系，而觀測值$\mathbf z_k$是LaserScan局部座標系。
  • 根據觀測值計算$R$：
    
• 卡爾曼濾波器循環（Kalman Filter Cycle）
  
  • 測量預測（Measurement Prediction）：經常是座標系變換（coordinate frame transform），狀態是基於全局/世界座標系，而觀測是基於局部Sensor座標系。
  • 數據關聯（Data Association）：如果有多個待估計的狀態或觀測值，需要把正確的預測值和觀測值進行配對，即一個預測值與哪一個觀測值相對應，以輸出正確的結果。通過統計兼容性測試來判斷預測值$i$與觀測值$j$之間的配置是正正確的。
    

2.3.2 EKF (Extended Kalman Filter 非線性)

• $\mathbf x_k 或 \mathbf z_k$：是非線性組合

• 對於非線性變換，其輸入爲高斯分佈，其輸出不再爲高斯分佈

• 通過一階Taylor展開進行近似

• 非線性函數一階誤差傳播（一維變量和多維變量）
  

• EKF公式
  

2.3.3 UKF (Unscented Kalman Filter 非線性)

• 定義：對非線性模型一種效果更好的線性化方式（與Taylor展開比較）
• 實現方法：
  • 計算一個Sigma點集合
    
  • 通過非線性函數變換每個Sigma點 (所以叫做無損變換UT: Unscented Transform)
    
  • 根據變換後的點及對應的權重計算高斯分佈 (而不像EFK哪樣在均值附近進行局部線性化)
    
• EKF變換過程如下：
  
• Sigma點($\chi^{[i]}, \omega^{[i]}$)的符合以下要求，且$\chi^{[i]}, \omega^{[i]}$沒有唯一解
  
• 計算Sigma點 $\chi^{[i]}$
  
• 計算權重$\omega^{[i]}$
  
• 計算要求的高斯分佈
  
• 無損變換參數
  

2.3.4 Particle Filter（粒子濾波）

• 如果數據的分佈是任意形式的，則不能採用高斯模型假設
• 使用基於樣本的不確定性表示，粒子過濾器（PF）是遞歸貝葉斯過濾器的實現，不對底層分佈和系統模型做任何假設

3. 數學基礎

3.1 概率論與統計學的區別

• 概率論：研究一個$\color{red}{白盒}$，即知道盒子內的詳細內容（如有幾個白球，有幾個黑球，也就是常說的概率分佈），然後計算下一個摸出白球的概率。
• 統計學：研究一個$\color{red}{黑盒}$，即不知道盒子內的詳細內容，只知道每次摸出來的是白球或黑球，其主要任務是根據實驗結果，來估計盒子內的詳細內容。
• 統計推斷：做統計推斷一般都需要對那個黑盒子做各種各樣的假設（即它符合什麼分佈），這些假設都是概率模型，統計推斷就是$\color{red}{估計這些模型的參數}$。

3.2 一維離散隨機變量（均值、方差、標準差）

• 描述對象：一個隨機變量（即一個特徵）的多個樣本
• 樣本（抽樣）：即有限個實驗樣本，其樣本數爲n
• 總體：所有樣本（即接近於隨機變量本身的概率分佈），設總體數爲N(接近於無窮大)
• 樣本方差中爲什麼除以$n-1$?
  • 使得樣本方差更能反映總體方差，因爲樣本範圍內可能不包含總體均值，所以樣本方差如果以樣本個數n作爲分母，就往往比總體方差小一點。
    | 概念 |總體（概率分佈） | 樣本 |
    | ------------- |:-------------? -----?
    | 均值(mean) | $\mu=\frac{1}{N}\sum_{i=1}^NX_i $ | $\overline{x} = \frac{1}{n}\sum_{i=1}^nx_i$ |
    | 方差(variance) | $\sigma^2=\frac{1}{N}\sum_{i=1}^N(X_i-\mu)^2$ | 總體樣本的無偏見估計$S^2=\frac{1}{n-1}\sum_{i=1}^n(x_i-\bar{x})^2$ |
    | 標準差(standard deviation) | $\sigma=\sqrt{\sigma^2}=\sqrt{\frac{1}{N}\sum_{i=1}^N(X_i-\mu)^2}$ | $S=\sqrt{S^2}=\sqrt{\frac{1}{n-1}\sum_{i=1}^n(x_i-\bar{x})^2}$|
    | 協方差(covariance) |$Cov(X,Y) = \frac{1}{N}\sum_{i=0}^N(X-\mu_x)(Y-\mu_y)$ |$Cov(x,y)=\frac{1}{n-1}\sum_{i=1}^n(x_i-\bar{x})(y_i-\bar{y})$ |
    | 相關係數（correlation coefficient） |$\rho_{X,Y}=\frac{Cov(X,Y)}{\sigma_X \sigma_Y}$ |$\rho_{x,y}=\frac{Cov(x,y)}{S_x S_y}$ |
    | 協方差矩陣(covariance matrix) | | |

3.2.1 數學期望(Expectation)

• 別名：數學期望或期望 （Expectation)
• 物理意義：描述一個特徵（即一個隨機變量）的$\color{red}{平均水平}$，需要多個樣本纔有意義，但不能反映樣本個體情況
• E(隨機變量)的計算公式：在概率論和統計學中，均值(mean)（或數學期望，亦簡稱期望）是試驗中每次可能結果的概率乘以其結果的總和。嚴格的定義如下：
  $\mu=E[x] =\sum_{i=0}^\infty x_i \cdot p_i \approx\frac{1}{N}\sum_{i=1}^Nx_i \qquad(X是離散隨機變量)$
  • $x_i$：表示一維隨機變量X的一個觀測樣本值
  • $p_i$：表示$x_i$發生的概率
    $\mu = E[x] = \int x \cdot p(x)\,dx \qquad (X是連續隨機變量)$
  • $p(x)$：隨機變量$X$的概率密度函數(PDF)
• E(隨機變量的函數)的計算公式:
  $E[f(x)] = \sum f(x) \cdot p(x) \qquad (x是離散隨機變量)$
  $E[f(x)] = \int f(x) \cdot p(x)\, dx \qquad (x是連續隨機變量)$
  $E[f(x,y)] = \iint f(x,y) \cdot p(x,y)\, dx\, dy \qquad (x,y 是連續隨機變量)$
• 各種常用數學期望的專用名稱
  
• 數學期望（均值）和算術平均值（平均數）的關係
  • 平均數：是根據實驗結果統計得到的隨機變量樣本計算出來的算術平均值，$\color{red}{和實驗本身有關}$，其計算公式爲：
    $\overline{x} = \frac{1}{n}\sum_{i=1}^nX_i$
  • 數學期望（均值）：是完全由隨機變量的概率分佈所確定的，$\color{red}{和實驗本身無關}$；
  • 二者的關係：實驗的次數多少是可以改變平均數的，而在你的分佈不變的情況下，期望是不變的。 如果我們能進行無窮次隨機實驗並計算出其樣本的平均數的話，那麼這個平均數其實就是期望。
• 性質
  • 以下設$a$爲常數，$x、y$爲隨機變量
  • $E[a] = a$
  • $E[a \cdot f(x)]=aE[f(x)]$
  • $E[f(x)+f(y))=E[f(x)]+E[f(y)]$
  • $x與y相互獨立，E[f(x) \cdot f(y)]=E[f(x)] \cdot E[f(y)]$

3.2.2 方差(variance)

• 方差：描述一個特徵（即一個隨機變量）的$\color{red}{離散程度}$，即描述樣本集合中的各個樣本點到均值的距離的平均。
• 物理意義：在概率論中，方差用來度量隨機變量和其數學期望（即均值）之間的偏離程度。
• 計算公式：
  $\sigma^2=D[x]=Var[x]=E[(x-E[x])^2] \approx \frac{1}{N}\sum_{i=1}^N(x_i-\mu_x)^2 \quad (x爲離散隨機劉變量是)$
  $\sigma^2=D[x]=Var[x]=E[(x-E[x])^2] = \int (x - \mu_x)^2 \cdot p(x) \, dx \quad (x爲連續隨機劉變量是)$
  $Var(x) = E[x^2] - (E[x])^2$
  • $x$：表示一維隨機變量
  • $E[x]$：表示$x$的均值(即期望）
  • $D[x]、Var[x]$：是每個樣本值與全體樣本均值之差的平方的平均數。
• 方差的性質：
  • 以下設a爲常數，x、y爲隨機變量
  • $D[a] = 0$
  • $D[ax] = a^2D[x]$
  • $D[x+a] = D[x]$
  • $D[x{\pm}y]=D[x]+D[y] \pm 2Cov(x,y)$
  • $x與y相互獨立：D[x +y]=D[x]+D[y]$
  • $D[x]=0的充要條件是x以概率1取常數E[x], 即P(x=E[x])=1$
  • $D[ax+by]=a^2D[x]+b^2D[y]+2abCov(x,y)$

3.2.3 標準差/均方差

• 物理意義：反映一個數據集的離散程度
• 計算公式：
  $\sigma=\sqrt {D[x]}$
• 標準差比方差的優勢：
  • 和原始數值的單位一致，不需要使用單位的平方
  • 標準差可以計算鍾型曲線（正態分佈）的中心值臨近區間的概率值，根據正態分佈定義，中心值的正負n倍 $\sigma$ 構成的區間對應不同的概率值．在正態分佈中，正負一個標準差之內所佔比率爲全部數值之 68。2%；正負兩個標準差之內（深藍，藍）的比率合起來爲 95.4%；正負三個標準差之內（深藍，藍，淺藍）的比率合起來爲 99.6%。
    
  • 在大樣本中一般使用樣本的標準差近似代替總體的標準差，儘管樣本標準差並不是理論上的無偏值，小樣本中偏差會比較大，但仍然可以通過t分佈模型等方法去估算。

3.2 多維隨機變量（協方差、相關係數、協方差矩陣）

• 均值、方差、標準差：描述了一個隨機變量多個樣本的統計屬性，若一個對象有多個特徵（多個隨機變量），如何描述多個隨機變量之間的關係呢？這正是協方差的用武之地。

3.2.1 協方差

• 協方差用途：用於描述兩個隨機變量在變化過程中是同方向變化，還是反方向變化？同向或反向的程度如何？
  • 在你變大的時刻，我也變大，說明兩個隨機變量是同向變化的，這時協方差是正的。
  • 在你變大的時刻，我變小，說明兩個隨機變量是反向變化的，這時協方差就是負的。
  • 從數值來看，協方差的數值越大，兩個隨機變量同向程度也就越大，在本質上，協方差並不能很好地描述同向或反向程度如何，而相關係數反而能更好地描述同向或反向的程度。
• 協方差公式
  $Cov(X,Y) = E[(X-E(X))(Y-E(Y))]\approx E[(X-\mu_x)(Y-\mu_y)]=\frac{1}{N}\sum_{i=0}^N(X-\mu_x)(Y-\mu_y)$
• $X、Y$：爲兩個隨機變量
• 若$X和Y$爲同一個隨機變量，則$Cov(X,Y)$爲$X$的方差，即$Cov(X,Y)=\sigma^2$
• 同向變化
  
  • 從上圖可知，$X、Y$均沿着各自的均值上下波動，且變化明顯是同向的
  • 在任意時刻$t_i$，$(X-\mu_x)與(Y-\mu_y)$正負符號相同，所以其乘積爲正；然後所有乘積相加，其結果仍然爲正
• 反向變化
  
  • 從上圖可知，$X、Y$均沿着各自的均值上下波動，且變化明顯是反向的
  • 在任意時刻$t_i$，$(X-\mu_x)與(Y-\mu_y)$正負符號相反，所以其乘積爲負；然後所有乘積相加，其結果仍然爲負
• 在實際應用中，樣本數據不可能這麼有規律，會出現$(X-\mu_x)與(Y-\mu_y)$時而爲正，時而爲負，我們關注的重點是總體樣本，而不是個體樣本，只要其最後的$Con(X,Y)$爲正，則表明$X、Y$的同向變化是大趨勢，且值(正數)越大，則同向程度越高；若爲負，則表明$X、Y$的反向變化是大趨勢，且值(負數)越小，則反向程度越高。

3.2.2 相關係數

• 相關係數公式
  $\rho_{X,Y}=\frac{Cov(X,Y)}{\sigma_X \sigma_Y}$

  • 即用$X、Y$的協方差除以X的標準差和Y的標準差。
  • 相關係數：可以看作是經過歸一化之後特殊協方差
  • 即值範圍：$[-1，1]$
  • 反映兩個隨機變量變化時是同向還是反向變化，如果同向變化就爲正，反幾變化變爲負。
  • 它消除了兩個隨機變量變化幅度的影響，從而單純地反映兩個隨機變量每單位變化時的相似程度。

• 相關係數用途
  

  • 從上圖可知，情況1與情況2在任意時刻都是同向變化，且均值相同
  • 很明顯，情況1的協方差大於情況2的協方差，因爲情況1中的$(X-\mu_x)$大於情況2中的$(X-\mu_x)$
  • 從本質上分析，情況1與情況2的同向程度相同，但協方差卻不一樣，所以協方差描述同向程度是不準確的，因爲它受變化幅度的影響
  • 而相關係數$\rho_{x,y}$卻是相同的，所以相關係數能很好地刻畫同向程度，剔除了變化幅度的影響

• 協方差與相關係數

  • 協方差受變化幅度的影響
  • 相關係數不受變化幅度的影響，能很好地描述同向或反向程度

• 相關係統各個值域的含義

  • 相關係數爲1：表明兩個隨機變量變化時的正向相似度最大，二者變化的倍數（即$Y=aX 且a&gt;0$）相同（你變大一倍，我也變大一倍；你變小一倍，我也變小一倍），即完全相關（以X、Y爲橫縱座標軸，可以畫出一條斜率爲正的直線，所以X、Y是線性關係的）
  • 相關係數爲[0,1)：隨着相關係數減小，兩個隨機變量變化時的相似度也變小，當爲0時，兩個變量的變化過程沒有任何相似度，即變量無關。
  • 相關係數爲(-1,0]：當相關係數小於0，兩個隨機變量開始出現反向的相似度，隨着相關係數減小，反向相似度會逐漸變大
  • 相關係數爲-1：表明兩個隨機變量變化時的反向相似度最大，二都變化的倍數相反($Y=bX 且b&lt;0$），即你變大一倍，我變小一倍；你變小一倍，我變大一倍。即完全負相關（以X、Y爲橫縱座標軸，可以畫出一條斜率爲負的直線，所以X、Y是線性關係的）

3.2.3 協方差矩陣

• 兩個隨機變量間可以計算其協方差，以描述其同向或反向程度，若一個對象包含有$n$個隨機變量，如何描述其任意兩個隨機變量的協方差呢？ 答案是$\color{red}{協方差矩陣}$。

• 描述對象：協方差矩陣計算的是不同維度(即不同隨機變量)之間的協方差，而不是不同樣本之間的。

• 定義：

  • 樣本有n個隨機變量，協方差矩陣就是這n個隨機變量中任意兩個隨機變量的協方差組成的矩陣
  • 矩陣中的數據按行排列與按列排列求出的協方差矩陣是不同的，這裏默認數據是按行排列。即每一行是一觀察樣本(observation sample)，那麼每一列就是一個隨機變量。
  • 記$X_{m \times n}$：爲有m個樣本，且每個樣本有n個隨機變量的矩陣:
    $X_{m \times n} = \begin{bmatrix} a_{11} &amp; a_{12} &amp; \cdots &amp; a_{1n} \\ a_{21} &amp; a_{22} &amp; \cdots &amp; a_{2n} \\ \vdots &amp; \vdots &amp; \ddots &amp; \vdots \\ a_{m1} &amp; a_{m2} &amp; \cdots &amp; a_{mn} \\ \end{bmatrix} = \begin{bmatrix} c_1 &amp; c_2 &amp; \cdots &amp; c_n \end{bmatrix}$
  • $c_1$：爲第一列隨機變量的$m$個樣本值，它是一個列向量，且爲$c_{m \times 1}$
  • 協方差矩陣定義爲：
    $\sum = covMatrix = \frac{1}{m-1} \begin{bmatrix} Cov(c_1, c_1) &amp; Cov(c_1,c_2) &amp; \cdots &amp; Cov(c_1, c_n) \\ Cov(c_2, c_1) &amp; Cov(c_2,c_2) &amp; \cdots &amp; Cov(c_2, c_n) \\ \vdots &amp; \vdots &amp; \ddots &amp; \vdots \\ Cov(c_n, c_1) &amp; Cov(c_n,c_2) &amp; \cdots &amp; Cov(c_n, c_n) \\ \end{bmatrix}$
  • 協方差矩陣的維度等於樣本中隨機變量的個數，即每一個樣本的維度。在某些場合前邊也會出現 1 / m，而不是 1 / (m - 1)。

• 計算方法：先讓樣本矩陣（矩陣的一行爲一個樣本）中心化(矩陣記作$X_D$)，即每一維度減去該維度的均值，使每一維度上的均值爲0，然後直接用中心化的樣本矩陣的轉置($X^T$)乘上它自己($X$)，然後除以樣本數(N)即可。
  $X_D=X_{Dm \times n} = \begin{bmatrix} cd_1 &amp; cd_2 &amp; \cdots &amp; cd_n \end{bmatrix}$
  $\overline{x_j} = \frac{1}{m}\sum_{i=1}^ma_{i,j } \qquad cd_{j} = \begin{bmatrix} a_{1,j} - \overline{x_j} \\ a_{2,j} - \overline{x_j} \\ \vdots \\ a_{m,j} - \overline{x_j} \\ \end{bmatrix}$
  $\sum=covMatrix=\frac{1}{m-1}X_D^TX_D$

• 性質

  • 協方差矩陣始終是一個對稱矩陣，其對角線上是方差，非對角線上是協方差。
  • 協方差矩陣對角線上的元素其實就是對應隨機變量的方差：$Cov(X,X)=Var(X)=\sigma^2$
  • 對於隨機變量$X, Y$, 則有$Cov(X,Y)=Cov(Y,X)$

• 協方差矩陣定義數據形狀
  

  • 協方差矩陣定義了我們數據的傳播（方差）和方向（協方差）。

###2.2.4協方差矩陣的特徵值分解

• 協方差矩陣定義了我們數據的傳播（方差）和方向（協方差）。因此，如果我們想用一個向量和它的大小來表示協方差矩陣，我們應該簡單地嘗試找到指向數據最大傳播方向上的向量，其大小等於這個方向上的傳播（方差）。
• 協方差矩陣的最大特徵向量總是指向數據最大方差的方向，並且該向量的幅度等於相應的特徵值。第二大特徵向量總是正交於最大特徵向量，並指向第二大數據的傳播方向。
• 協方差矩陣是對角矩陣:
  • 當協方差是零時，方差必須等於特徵值λ。如下圖所示，特徵向量用綠色和品紅色表示，特徵值顯然等於協方差矩陣的方差分量。
    
• 協方差矩陣不是對角矩陣
  • 當協方差不爲零，那麼情況稍微更復雜一些。特徵值仍代表數據最大傳播方向的方差大小，協方差矩陣的方差分量仍然表示x軸和y軸方向上的方差大小。但是，因爲數據不是軸對齊的，所以這些值不再與上圖所示的相同。
    
• 特徵值：表示沿特徵向量方向數據的方差，而協方差矩陣的方差分量表示沿軸的傳播。如果沒有協方差，則這兩個值是相等的。
• 特徵向量表示數據最大方差的方向，特徵值表示特徵向量方向方差的幅度。
• 協方差矩陣與白色不相關數據的線性變換有直接的關係，此線性變換完全由數據的特徵向量和特徵值確定。而特徵向量表示旋轉矩陣，特徵值對應於每個維度上縮放因子的平方。
  

[參考]
http://ais.informatik.uni-freiburg.de/teaching/ss18/robotics/slides
http://ais.informatik.uni-freiburg.de/teaching/ss11/robotics/slides/09b-errorprop.pdf
EKF Tutorial (直觀理解，淺顯易懂)
Unscented Kalman Filter