跳到主要內容

[最佳控制] 線性系統的最佳參數估計 - Kalman Filter (1)

此文章基於前篇 [最佳控制] 線性系統的最佳參數估計 - Kalman Filter (0);強烈建議讀者先參閱前篇再行閱讀此文。此文將利用 計算 probability density 方式來求解 Kalman filter。


考慮離散時間動態系統
\[
x(k+1) = Ax(k) + w(k);\;\; y(k) = Cx(k) + v(k)
\]且 $w \sim N(0,Q)$ 為製程雜訊 process noise 或稱 干擾 process disturbance;$v \sim N(0,R)$ 為量測雜訊 (measurement noise) ; $x(0) \sim N(\bar x(0), Q(0))$;$x \in \mathbb{R}^n, A \in \mathbb{R}^{n \times n}, C \in \mathbb{R}^{p \times n}, y \in \mathbb{R}^p$,Kalman filter 便是在試圖回答:假設狀態未知我們只能拿到量測輸出,則最佳的狀態估計該是如何?


First Step : $k=0$
假設 初始狀態 $x(0)$ 為 mean $\bar x(0)$ 且 convariance matrix $Q(0)$ 的 normal distrbuted 隨機向量,亦即
\[
x(0) \sim N(\bar x(0), Q(0))
\]接著獲得 初始 (受雜訊污染的) 量測輸出 $y(0)$ 滿足下式
\[
y(0) = C x(0) + v(0)
\]其中 $v(0) \sim N(0, R)$ 為 雜訊 (measurement noise)。

我們的目標:獲得 conditional density $p_{x(0)|y(0)}(x(0) | y(0))$ ,則我們的狀態估計 $\hat x$ 即可透過此 conditional density 求得
$$\hat x := \arg \max_x p_{x(0)|y(0)}(x(0) | y(0))$$

Comments:
1. 若未知 $\bar x(0)$ 或者 $Q(0)$ 則我們通常選 $\bar x(0) :=0$ 且 $Q(0)$ 很大 來表示我們對初始狀態所知甚少 (noninformative prior)。
2. 若量測過程中受到大的雜訊,則我們會給予較大的 $R$。若量測過程十分精確沒有太多雜訊汙染我們的輸出 $y$ 則 $R$ 較小。
3. Conditional density 描述了在我們獲得 初始量測輸出 $y(0)$ 之後我們對於 $x(0)$ 的了解。


現在回歸我們的目標,究竟該如何推得 $p_{x(0)|y(0)}(x(0) | y(0))$ ?
首先考慮 $(x(0), y(0))$ 如下
\[\left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{y\left( 0 \right)}
\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{v\left( 0 \right)}
\end{array}} \right]
\]假設 $v(0)$ 與 $x(0)$ 彼此互為獨立。注意到  $x(0), v(0)$ 為 joint normal 亦即
\[\left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{v\left( 0 \right)}
\end{array}} \right] \sim N\left( {\left[ {\begin{array}{*{20}{c}}
{\bar x\left( 0 \right)}\\
0
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
{Q\left( 0 \right)}&0\\
0&{{R}}
\end{array}} \right]} \right)\]
上述 $[x(0) \;\; y(0)]$ 為  $[x(0) \;\; v(0)]$ 線性轉換 且 故我們可以馬上知道
\[\begin{array}{l}
\left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{y\left( 0 \right)}
\end{array}} \right]\sim N\left( {\left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{\bar x\left( 0 \right)}\\
0
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{Q\left( 0 \right)}&0\\
0&R
\end{array}} \right]{{\left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]}^T}} \right)\\
 \Rightarrow \left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{y\left( 0 \right)}
\end{array}} \right] \sim N\left( {\left[ {\begin{array}{*{20}{c}}
{\bar x\left( 0 \right)}\\
{C\bar x\left( 0 \right)}
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{Q\left( 0 \right)}&0\\
0&R
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
I&{{C^T}}\\
0&I
\end{array}} \right]} \right)\\
 \Rightarrow \left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{y\left( 0 \right)}
\end{array}} \right]\sim N\left( {\left[ {\begin{array}{*{20}{c}}
{\bar x\left( 0 \right)}\\
{C\bar x\left( 0 \right)}
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
{Q\left( 0 \right)}&{Q\left( 0 \right){C^T}}\\
{CQ\left( 0 \right)}&{CQ\left( 0 \right){C^T} + R}
\end{array}} \right]} \right)
\end{array}\]現在有了 $x(0), y(0)$ 的 joint density, 注意到此 joint density 為 normal,故若要計算 conditonal density of $x(0)$ given $y(0)$,亦即 $p_{x(0)|y(0)}(x(0)|y(0))$ ;則我們可以使用前述文章討論的 FACT 3 來求得,亦即
\[
p_{x(0)|y(0)}(x(0)|y(0)) = n(x(0), m, P)
\]其中
\[\left\{ {\begin{array}{*{20}{l}}
\begin{array}{l}
m = \bar x\left( 0 \right) + L\left( 0 \right)(y\left( 0 \right) - C\bar x\left( 0 \right))\\
L\left( 0 \right): = Q\left( 0 \right){C^T}\left( {CQ\left( 0 \right){C^T} + R} \right)_{}^{ - 1}
\end{array}\\
{P = Q\left( 0 \right) - Q\left( 0 \right){C^T}\left( {CQ\left( 0 \right){C^T} + R} \right)_{}^{ - 1}CQ\left( 0 \right)}
\end{array}} \right.\]則 optimal state estimation  $\hat x$ 及為 具有最大 conditional density 的 $x(0)$;對於 normal distribution 而言,此 $\hat x$ 剛好為 mean;故我們選 $\hat x(0) := m$;且上式中 $P := P(0)$ 表示獲得 量測輸出 $y(0)$ 之後的 variance

Next Step: State Prediction
考慮現在狀態從 $k=0$ 移動到 $k=1$,我們有
\[
x(1) = Ax(0) + w(0)
\]亦可將其寫成線性轉換型式:
\[x\left( 1 \right) = \left[ {\begin{array}{*{20}{c}}
A&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{w\left( 0 \right)}
\end{array}} \right]\]其中 $w(0) \sim N(0, Q)$ 為 系統干擾 (disturbance) 或稱 製程雜訊 (process noise)。若 狀態受到大的外部擾動,則可以想見 會有較大的 $Q$ convaraince matrix。同理若外部干擾較小則有較小的雜訊。

故我們目標:要計算 conditional density $p_{x(1)|y(0)}(x(1),y(0))$

現在我們需要 conditional on joint density $(x(0),w(0))$ given $y(0)$,假設 $w(0)$ 與 $x(0),v(0)$ 彼此互為獨立,則我們可寫
\[\left( {\left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{w\left( 0 \right)}
\end{array}} \right]|y\left( 0 \right)} \right)\sim N\left( {\left[ {\begin{array}{*{20}{c}}
{\hat x\left( 0 \right)}\\
0
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
{P\left( 0 \right)}&0\\
0&Q
\end{array}} \right]} \right)\]故現在利用 前述文章討論的 FACT 3' 來求得  conditional density,亦即
 \[\begin{array}{l}
\left( {\left[ {\begin{array}{*{20}{c}}
{x\left( 0 \right)}\\
{w\left( 0 \right)}
\end{array}} \right]|y\left( 0 \right)} \right)\sim  N\left( {\left[ {\begin{array}{*{20}{c}}
{\hat x\left( 0 \right)}\\
0
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
{P\left( 0 \right)}&0\\
0&Q
\end{array}} \right]} \right)\\
 \Rightarrow \left( {x\left( 1 \right)|y\left( 0 \right)} \right)\sim N\left( {\left[ {\begin{array}{*{20}{c}}
A&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{\hat x\left( 0 \right)}\\
0
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
A&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{P\left( 0 \right)}&0\\
0&Q
\end{array}} \right]{{\left[ {\begin{array}{*{20}{c}}
A&I
\end{array}} \right]}^T}} \right)\\
 \Rightarrow \left( {x\left( 1 \right)|y\left( 0 \right)} \right)\sim N\left( {A\hat x\left( 0 \right),AP\left( 0 \right){A^T} + Q} \right)
\end{array}\]故 conditional density 仍為 normal
\[
p_{x(1)|y(0)}(x(1)|y(0)) = n(x(1), \hat x^-(1), P^-(1))
\]其中
\[\left\{ \begin{array}{l}
{{\hat x}^ - }\left( 1 \right) = A\hat x\left( 0 \right)\\
{P^ - }\left( 1 \right) = AP\left( 0 \right){A^T} + Q
\end{array} \right.\]接著我們僅需 遞迴重複上述步驟 $k=2,3,4,...$ 即可。以下我們給出總結:

Summary
定義 量測輸出從初始 直到 時間 $k$ 則
\[
{\bf y}(k) := \{y(0),y(1),...,y(k)\}
\]在 時間 $k$ 時,conditional density with data ${\bf y}(k-1)$ 為 normal
\[
p_{x(k)| {\bf y}(k-1)}(x(k) | {\bf y}(k-1)) = n(x(k), \hat x^-(k),  P^-(k))
\]上述 mean 與 covariance matrix 有上標 $^-$ 號表示此估計為僅僅透過 量測 ${\bf y}(k-1)$ 的輸出,並未獲得 $k$ 時刻的量測輸出。 (表示用過去 $k-1$ 資料 預測 $k$ 的狀態! ) 注意到在 $k=0$,遞迴起始於 $\hat x^- (0) = \bar x(0)$ 與 $P^-(0) = Q(0)$。

接著我們會獲得 $y(k)$ 滿足
\[\left[ {\begin{array}{*{20}{c}}
{x\left( k \right)}\\
{y\left( k \right)}
\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{x\left( k \right)}\\
{v\left( k \right)}
\end{array}} \right]
\]上式表示線性轉換。由於 量測雜訊 $v(k)$ 與 $x(k)$ 以及 ${\bf y}(k-1)$ 彼此獨立,故我們有  density of $(x(k), v(k))$
\[\underbrace {\left[ {\begin{array}{*{20}{c}}
{x\left( k \right)}\\
{v\left( k \right)}
\end{array}} \right]}_{ = \left( {\left[ {\begin{array}{*{20}{c}}
{x\left( k \right)}\\
{v\left( k \right)}
\end{array}} \right]|{\bf{y}}\left( {k - 1} \right)} \right)}\sim N\left( {\left[ {\begin{array}{*{20}{c}}
{{{\hat x}^ - }\left( k \right)}\\
0
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
{{P^ - }\left( k \right)}&0\\
0&R
\end{array}} \right]} \right)\]透過線性轉換結果,可得
\[\begin{array}{l}
\left[ {\begin{array}{*{20}{c}}
{x\left( k \right)}\\
{y\left( k \right)}
\end{array}} \right]\sim N\left( {\left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{{{\hat x}^ - }\left( k \right)}\\
0
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{{P^ - }\left( k \right)}&0\\
0&R
\end{array}} \right]{{\left[ {\begin{array}{*{20}{c}}
I&0\\
C&I
\end{array}} \right]}^T}} \right)\\
 \Rightarrow \left[ {\begin{array}{*{20}{c}}
{x\left( k \right)}\\
{y\left( k \right)}
\end{array}} \right]\sim N\left( {\left[ {\begin{array}{*{20}{c}}
{{{\hat x}^ - }\left( k \right)}\\
{C{{\hat x}^ - }\left( k \right)}
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
{{P^ - }\left( k \right)}&{{P^ - }\left( k \right){C^T}}\\
{C{P^ - }\left( k \right)}&{C{P^ - }\left( k \right){C^T} + R}
\end{array}} \right]} \right)
\end{array}\]注意到 $\{{\bf y}(k-1), y(k)\} \equiv {\bf y}(k)$ 故利用 conditional density 結果可得
\[
p_{x(k)|{\bf y} (k)}(x(k)| {\bf y}(k)) = n(x(k), \hat x(k), P(k))
\]其中
\[\left\{ {\begin{array}{*{20}{l}}
\begin{array}{l}
\hat x\left( k \right) = {{\hat x}^ - }\left( k \right) + L\left( k \right)(y\left( k \right) - C{{\hat x}^ - }\left( k \right))\\
L\left( k \right) = {P^ - }\left( k \right){C^T}\left( {C{P^ - }\left( k \right){C^T} + R} \right)_{}^{ - 1}
\end{array}\\
{P\left( k \right) = {P^ - }\left( k \right) - {P^ - }\left( k \right){C^T}\left( {C{P^ - }\left( k \right){C^T} + R} \right)_{}^{ - 1}C{P^ - }\left( k \right)}
\end{array}} \right.\]現在我們用下列模型來預估 基於 $k$ 時刻量測值 預估 $k+1$ 時刻
\[x\left( {k + 1} \right) = \left[ {\begin{array}{*{20}{c}}
A&I
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{x\left( k \right)}\\
{w\left( k \right)}
\end{array}} \right]\]由於 $w(k)$ 與 $x(k)$ 以及 $\bf y$$(k)$ 彼此獨立,故 joint density of $(x(k),w(k))$ 可寫為
\[\left[ {\begin{array}{*{20}{c}}
{x\left( k \right)}\\
{w\left( k \right)}
\end{array}} \right]\sim N\left( {\left[ {\begin{array}{*{20}{c}}
{\hat x\left( k \right)}\\
0
\end{array}} \right],\left[ {\begin{array}{*{20}{c}}
{P\left( k \right)}&0\\
0&Q
\end{array}} \right]} \right)\]故 conditional density 為
\[{p_{x\left( {k + 1} \right)|{\bf{y}}\left( k \right)}}\left( {x\left( {k + 1} \right)|{\bf{y}}\left( k \right)} \right) = n\left( {x\left( {k + 1} \right),{{\hat x}^ - }\left( {k + 1} \right),{P^ - }\left( {k + 1} \right)} \right)\]其中
\[\left\{ \begin{array}{l}
{{\hat x}^ - }\left( {k + 1} \right) = A\hat x\left( k \right)\\
{P^ - }\left( {k + 1} \right) = AP\left( k \right){A^T} + Q
\end{array} \right.\]

留言

這個網誌中的熱門文章

[數學分析] 什麼是若且唯若 "if and only if"

數學上的 if and only if  ( 此文不討論邏輯學中的 if and only if,只討論數學上的 if and only if。) 中文翻譯叫做  若且唯若 (or 當且僅當) , 記得當初剛接觸這個詞彙的時候,我是完全不明白到底是甚麼意思,查了翻譯也是愛莫能助,畢竟有翻跟沒翻一樣,都是有看沒有懂。 在數學上如果看到 if and only if  這類的句子,其實是表示一種 雙條件句 ,通常可以直接將其視為" 定義(Definition)" 待之,今天要分享的是這樣的一個句子如何用比較直觀的方法去看他 假設我們現在有 兩個邏輯陳述句 A 與  B. 注意到,在此我們不必考慮這兩個陳述句到底是什麼,想表達什麼,或者到底是否為真(true),這些都不重要。只要知道是兩個陳述即可。 現在,考慮新的陳述:  "A if and only if B" 好了,現在主角登場,我們可以怎麼看待這個句子呢? 事實上我們可以很直覺的把這句子拆成兩部分看待,也就是 "( A if B ) and ( A only if B )" 那麼先針對第一個部分  A if B  來看, 其實這句就是說  if B then A, 更直白一點就是 "if B is true, then A is also true".  在數學上等價可以寫為 "B implies A" .  或者更常用一個箭頭符號來表示 "B $\Rightarrow$  A"  現在針對第二個部分  A only if B 此句意指  "If B is not true, then A is also not true". 所以如果已知 A is true,  那麼按照上句不難推得 B is also true 也就是說  A only if B  等價為 "If A is true then B is also true". 同樣,也可以寫作   "A implies B"   或者用箭頭表示  "A   $\Rightarrow$     B".

[數學分析] 淺談各種基本範數 (Norm)

這次要介紹的是數學上一個重要的概念: Norm: 一般翻譯成 範數 (在英語中 norm 有規範的意思,比如我們說normalization就是把某種東西/物品/事件 做 正規化,也就是加上規範使其正常化),不過個人認為其實翻譯成 範數 也是看不懂的...這邊建議把 Norm 想成長度就好 (事實上norm是長度的抽象推廣), 也許讀者會認為好端端的長度不用,為何又要發明一個 norm 來自討苦吃?? 既抽象又艱澀。 事實上想法是這樣的: 比如說現在想要比較兩個數字 $3$ , $5$ 之間的大小,則我們可以馬上知道 $ 3 < 5 $;同樣的,如果再考慮小數與無理數如 $1.8753$ 與 $\pi$,我們仍然可以比較大小 $1.8753 < \pi = 3.1415...$ 故可以發現我們有辦法對 "純量" 做明確的比大小,WHY? 因為前述例子中 $3$, $5$, $1.8753$ or $\pi$ 其各自的大小有辦法被 "measure "! 但是如果是現在考慮的是一組數字 我們如何去measure 其大小呢?? 比如說 \[x:=[1, -2, 0.1, 0 ]^T \]上式的大小該是多少? 是 $1$? $-2$? $0.1$??? 再者如果更過分一點,我們考慮一個矩陣 \[A = \left[ {\begin{array}{*{20}{c}} 1&2\\ 3&4 \end{array}} \right] \],想要知道這個矩陣的大小又該怎麼辦?? 是 $1$ ? $2$ 還是 $4$ ?..其實現階段我們說不清楚。 也正是如此,可以發現我們確實需要新的 "長度" 的定義來幫助我們如何去 measure 矩陣/向量/甚至是函數的大小。 故此,我們首先定義甚麼是Norm,(也就是把 "長度" or "大小" 的本質抽離出來) ================== Definition: Norm 考慮 $V$ 為一個向量空間(Vector space),則我們說  Norm 為一個函數 $||\cdot|| : V \rightarrow \mathbb{R}$ 且滿足下列性質