狀態估計第二講:線性高斯系統的狀態估計問題

本講的核心問題就是線性系統卡爾曼濾波的推導。

離散時間的批量估計

線性高斯模型

運動方程：$x_k=A_{k-1}x_{k-1}+v_k+w_k,k=1,2……K$
觀測方程：$y_k=C_kx_k+n_k,k=0,1,2……K$
各個變量的意義：
系統狀態：$x_k\in\R^N$
初始狀態：$x_0\in\R^N{\sim} N(\check{x_0},\check{P_0})$
輸入：$v_k\in\R^N$
狀態轉移矩陣：$A_{k-1}$
過程噪聲：$w_k\in\R^N{\sim}N(0,Q_k)$
觀測矩陣：$C_K$
觀測噪聲：$n_k\in\R^N{\sim}N(0,R_k)$
觀測量：$y_k\in\R^N$
狀態估計問題： 通過初始狀態、各時刻的觀測數據、輸入數據，估計系統的真實狀態

最大後驗估計(MAP)

MAP問題：已知輸入和觀測，求最大概率的狀態
$\hat{x}=arg\underset{x}{max}p(x|y,v)$
用貝葉斯公式重寫上述公式
$\hat{x}=arg\underset{x}{max}\frac{p(y|x,v)*p(x|v)}{p(y|v)}=arg\underset{x}{max}p(x|v)p(y|x)$
由於各個時刻觀測、輸入的噪聲都是無關的，上面兩個項可以因式分解：
$p(x|v)=p(x_0|\check x_0)\prod_{k=1}^Kp(x_k,|x_{k-1},v_k)$
$p(y|x)=\prod_{k=0}^Kp(y_k|x_k)$
對目標函數取對數可得：
$\ln(p(y|x)p(x|v))=\ln p(x_0|\check{x_0})+\sum_{k=1}^K \ln p(x_k|x_{k-1},v_k)+\sum_{k=0}^K \ln p(y_k|x_k)$
將概率化成相應的數學形式有：
$\ln p(x_0|\check x_0)=- \frac{1}{2}(x_0-\check x_0)^T\check P_0^{-1}(x_0-\check x_0)- \frac{1}{2}\ln((2\pi)^Ndet\check P_0)$
$\ln p(x_k|x_{k-1},v_k)=-\frac{1}{2}(x_k-A_{k-1}x_{k-1}-v_k)^TQ_k^{-1}(x_k-A_{k-1}x_{k-1}-v_k)-\frac{1}{2}\ln((2\pi)^NdetQ_k)$
$\ln p(y_k|x_k)=- \frac{1}{2}(y_k-C_kx_k)^TR_k^{-1}(y_k-C_kx_k)- \frac{1}{2}\ln ((2\pi)^MdetR_k)$
去掉與$x$無關的項，定義如下等式：
$J_{v,k}(x)=\begin{cases} - \frac{1}{2}(x_0-\check x_0)^T\check P_0^{-1}(x_0-\check x_0) ,k=0\\ -\frac{1}{2}(x_k-A_{k-1}x_{k-1}-v_k)^TQ_k^{-1}(x_k-A_{k-1}x_{k-1}-v_k),k=1,……K \end{cases}$
$J_{y,k}(x)=- \frac{1}{2}(y_k-C_kx_k)^TR_k^{-1}(y_k-C_kx_k),k=0,……，K$
於是目標函數變成最小二乘問題：
$\hat x=\arg \underset{x}maxJ_x, J_x=\sum_{k=0}^K(J_{v,k}(x)+J_{y,k}(x))$
寫成更緊湊的矩陣形式：

把運動和觀測寫在一起：
$z=Hx+W$
提升形式的目標函數：
$J(x)=\frac {1}{2}(z-Hx)^T W^{-1}(z-Hx)$
該式是個二次的，求其最小值，只令自變量最小值導數爲0：
$\frac {\partial J(x)}{\partial {x^T}}=-H^TW^{-1}(z-H\hat x)=0$
$\Rightarrow(H^TW^{-1}H)\hat x=H^TW^{-1}z$

貝葉斯推斷

在LG(線性高斯)系統中，可以根據運動方程和觀測方程顯式寫出狀態變量分佈的變化過程。
單個時刻：
$x_k=A_{k-1}x_{k-1}+v_k+w_k$
提升形式：
$x=A(v+w)$
$A=\begin{bmatrix} 1 \\ A_0 & 1 \\ A_1A_0 & A_1 & 1 \\ \vdots &\vdots &\vdots \\ A_{K-1}…A_0 & A_{K-2}…A_1 & A_{K-2}…A_2 & … & 1 \\ A_{K-1}…A_0 &A_{K-1}…A_1 & A_{K-1}…A_2&…&A_{K-1} & 1\end{bmatrix}$
提升形式裏，右側只有v和w，容易求得其均值和協方差：
$\check x=E[x]=E[A(v+w)]=Av$
$\check P=E[(x-E[x])(x-E[x])^T]=AQA^T$
先驗部分寫爲:$p(x|v)=N(\check x,\check P)=N(Av,AQA^T)$
觀測模型：
單次觀測：$y_k=C_kx_k+n_k$
提升形式：$y=Cx+n,C=diag(C_0,C_1，……C_K)$
聯合分佈：
$p(x,y|v)=N(\begin{bmatrix} \check x \\ C\check x\end{bmatrix},\begin{bmatrix} \check P & \check PC^T \\ C\check P & C\check PC^T+R\end{bmatrix})$
由第一章的高斯推斷可得：
$p(x|v,y)=N(\check x+\check PC^T(C\check PC^T+R)^{-1}(y-C\check x),\check P- \check PC^T(C\check PC^T+R)^{-1}C\check P)$
帶入SMW式進行化簡可得：
$p(x|v,y)=N(\begin{matrix} (\underbrace{\check P^{-1}+C^TR^{-1}C)^{-1}(\check P^{-1}\check x+C^TR^{-1}y)} , &\underbrace {(\check P^{-1}+C^TR^{-1}C)^{-1})} \\ {均值\hat x} & 後驗協方差\hat P\end{matrix}$
均值部分：$(\check P^{-1}+C^TR^{-1}C)\hat x=\check P^{-1}\check x+C^TR^{-1}y$
代入$\check x=Av$和$\check P^{-1}=A^{-T}Q^{-1}A^{-1}$
得均值式：$(A^{-T}Q^{-1}A^{-1}+C^{T}R^{-1}C)\hat x=A^{-T}Q^{-1}v+C^{T}R^{-1}y$
由於A的結構，A逆具有特殊形式：
$A^{-1}=\begin{bmatrix} 1\\ -A_0 & 1 \\ &-A_1 & 1 \\ & & -A_2 & \ddots \\ & & \ddots & 1 \\ & & & -A_{K-1} & 1\end{bmatrix}$
按照均值式，定義矩陣:
$z=\begin{bmatrix} v \\ y\end{bmatrix},H=\begin{bmatrix} A^{-1} \\ C\end{bmatrix},W=\begin{bmatrix} Q & \\ &R\end{bmatrix}$
可得：$(H^TW^{-1}H)\hat x=H^TW^{-1}z$與MAP結果完全一致!
MAP結果和貝葉斯推斷結果一致，說明了什麼？
• MAP只關心達到最大後驗概率的一個點，這個點的狀態稱爲MAP估計。
• 而貝葉斯推斷寫出了p(x|y,v)的完整形式，它是一個高斯分佈，其均值與MAP估計相等；同時，給出了這個估計的協方差。
• 如果我們只關心狀態估計變量取值，那麼MAP給出了後驗分佈的模（Mode），貝葉斯推斷給出了均值。
• 而在LG系統中，二者是一樣的，使得這兩類方法給出了同樣的結果。
LG系統最優估計結果唯一的條件：$rank(H^TW^{-1}H)=N(K+1),N(K+1)表示x的維度$。
由於協方差矩陣的對稱正定性，即要求$rank(H^TH)=rank(H^T)=N(K+1)$
H的具體形式取決於問題有沒有0時刻的先驗條件。

離散時間的遞歸平滑算法

很多在線問題當中（比如定位），我們有上一個時刻的先驗估計，希望通過這個時刻的控制和觀測，計算這個時刻的狀態估計。這裏介紹遞歸解法。遞歸解法的基礎是批量問題的解法。
批量問題的核心是用Cholesky分解法求解方程$(H^TW^{-1}H)\hat x=H^TW^{-1}z$
Cholesky解方程的流程：
•Cholesky分解：$H^T W^{-1}H=LL^T$
• 先解：$Ld=H^TW^{-1}z$ 得到d，從上往下解；
• 再解： $L^T\hat x=d$得到最優狀態，從下往上解；
• 注意這種解法對一般線性方程也是有效的，不光是針對狀態估計問題
• 這兩步分別稱爲前向過程和後向過程（forward/backward）。
迭代法是建立在Cholesky基礎上，最終得到經典的RTS Smoother：
前向：$k=1,……,K$
$\check P_{k,f}=A_{k-1}\hat P_{k-1,f}A_{k-1}^T+Q_k$
$\check x_{k,f}=A_{k-1}\hat x_{k-1,f}+v_k$
$K_k=\check P_{k,f}C_k^T(C_k\check P_{k,f}C_k^T+R_k)^{-1}$
$\hat P_{k,f}=(1-K_kC_k)\check P_{k,f}$
$\hat x_{k,f}=\check x_{k,f}+K_k(y_k-C_k\check x_{k,f})$
後向：$k=K,……，1$
$\hat x_{k-1}=\hat x_{k-1,f}+\hat P_{k-1,f}A_{k-1}^T\check P_{k,f}^{-1}(\hat x_k-\check x_{k,f})$

離散時間的濾波算法

• RTS Smoother是無法在線運行的（非因果的Not causal）
• 它的後向迭代過程使用下個時刻的信息更新之前的估計
• 初始值中需要知道x(K)的後驗

利用MAP和貝葉斯推斷均可推導出卡爾曼濾波，在此只給出卡爾曼濾波最終形式：

關於卡爾曼濾波器的結論
• 卡爾曼濾波器給出了LG系統下最優線性無偏估計（Best Linear Unbiased Estimate, BLUE）
• 需要有初始狀態
• 卡爾曼濾波器即RTS Smoother的前向部分
• 在非線性場合下，我們會使用擴展卡爾曼濾波器（EKF），但此時MAP、貝葉斯推斷、EKF給出的結果均會不一樣