卡爾曼濾波

卡爾曼濾波本質(zhì)上是：非平穩(wěn)信號(hào)下的序貫線性最小均方誤差估計(jì)量

1贮缕、最小均方誤差

對(duì)于經(jīng)典的均方誤差（Mean Square Error, MSE）方法認(rèn)為赏殃，被估計(jì)量 $\boldsymbol{\theta}$ 是一個(gè)確定的未知常量财破，因此經(jīng)典MSE通過(guò)求解下式最小得到估計(jì)值
$\text{MSE}(\boldsymbol{\theta}) = \int \| \hat{\boldsymbol{\theta}} - \boldsymbol{\theta} \|^2 p(\boldsymbol{x}) d\boldsymbol{x}$

其中 $\hat{\boldsymbol{\theta}}$ 是觀測(cè)量 $\boldsymbol{x}$ 的函數(shù)成箫，即 $\hat{\boldsymbol{\theta}} = f(\boldsymbol{x})$

而當(dāng)估計(jì)量是一個(gè)隨機(jī)變量時(shí)，上述的結(jié)果并沒(méi)有考慮估計(jì)量 $\boldsymbol{\theta}$ 的先驗(yàn) $p(\boldsymbol{\theta})$ ，在添加先驗(yàn)信息后經(jīng)典MSE將會(huì)變成貝葉斯均方誤差（Bayes Mean Square Error, BMSE）漏麦，其表達(dá)式如下
$\text{BMSE}(\boldsymbol{\theta}) = \iint \| \hat{\boldsymbol{\theta}} - \boldsymbol{\theta} \|^2 p(\boldsymbol{x}, \boldsymbol{\theta}) d\boldsymbol{x} d\boldsymbol{\theta} =E(\| \hat{\boldsymbol{\theta}} - \boldsymbol{\theta} \|^2)$

同樣有 $\hat{\boldsymbol{\theta}} = f(\boldsymbol{x})$ ，其中期望是根據(jù) $p(\boldsymbol{x}, \boldsymbol{\theta})$ 來(lái)求解的况褪，使得BMSE最小的 $\hat{\boldsymbol{\theta}}$ 被稱為最小貝葉斯均方誤差估計(jì)量也被稱為最小均方誤差（Minimum Mean Square Error, MMSE）估計(jì)量

2撕贞、線性最小均方誤差

如果對(duì)函數(shù) $\hat{\boldsymbol{\theta}} = f(\boldsymbol{x})$ 做線性假設(shè)，即認(rèn)為估計(jì)量由觀測(cè) $\boldsymbol{x}$ 線性組合而成测垛，則有 $\hat{\boldsymbol{\theta}} = \boldsymbol{K} \boldsymbol{x} + \boldsymbol{t}$ 捏膨，其BMSE可以寫(xiě)成
$\text{BMSE} = E(\| \boldsymbol{K} \boldsymbol{x} + \boldsymbol{t} - \boldsymbol{\theta} \|^2)$

為了使得上式最小，對(duì) $\boldsymbol{t}$ 求導(dǎo)等于 $\boldsymbol{0}$ 可以得到
$\begin{aligned} \frac{\partial }{\partial\boldsymbol{t}}E(\| \boldsymbol{K} \boldsymbol{x} + \boldsymbol{t} - \boldsymbol{\theta} \|^2) =& 0 \\ \Rightarrow E(2\boldsymbol{t} + 2 \boldsymbol{K} \boldsymbol{x}- 2\boldsymbol{\theta}) =& 0 \\ \Rightarrow \boldsymbol{t} =& E(\boldsymbol{\theta}) -\boldsymbol{K} E(\boldsymbol{x}) \end{aligned}$

帶入原式中并且繼續(xù)對(duì) $\boldsymbol{K}$ 進(jìn)行求導(dǎo)
$\begin{aligned} \frac{\partial }{\partial\boldsymbol{K}}E(\| \boldsymbol{K} (\boldsymbol{x} - E(\boldsymbol{x})) - (\boldsymbol{\theta} - E(\boldsymbol{\theta})) \|^2) = 0 \\ \Rightarrow E(2\boldsymbol{K}(\boldsymbol{x} - E(\boldsymbol{x}))(\boldsymbol{x} - E(\boldsymbol{x}))^T - 2(\boldsymbol{\theta} - E(\boldsymbol{\theta})) (\boldsymbol{x} - E(\boldsymbol{x})^T) = 0 \\ \Rightarrow \boldsymbol{K} = E((\boldsymbol{\theta} - E(\boldsymbol{\theta})) (\boldsymbol{x} - E(\boldsymbol{x})^T)E((\boldsymbol{x} - E(\boldsymbol{x})) (\boldsymbol{x} - E(\boldsymbol{x})^T)^{-1} \\ \Rightarrow \boldsymbol{K} = \boldsymbol{C}_{\boldsymbol{\theta}\boldsymbol{x}}\boldsymbol{C}_{\boldsymbol{x}\boldsymbol{x}}^{-1} \end{aligned}$

最終求得
$\hat{\boldsymbol{\theta}} = E(\boldsymbol{\theta}) + \boldsymbol{C}_{\boldsymbol{\theta}\boldsymbol{x}}\boldsymbol{C}_{\boldsymbol{x}\boldsymbol{x}}^{-1} (\boldsymbol{x} - E(\boldsymbol{x}))$

在 $\boldsymbol{\theta}, \boldsymbol{x}$ 零均值的假設(shè)下食侮，即 $E(\boldsymbol{\theta}) = 0, E(\boldsymbol{x}) = 0$ 号涯，上式可以化簡(jiǎn)成
$\hat{\boldsymbol{\theta}} = \boldsymbol{C}_{\boldsymbol{\theta}\boldsymbol{x}}\boldsymbol{C}_{\boldsymbol{x}\boldsymbol{x}}^{-1} \boldsymbol{x}$

該估計(jì)量被稱為線性最小方差估計(jì)量（Linear Minimum Mean Square Error, LMMSE）

3、卡爾曼濾波

如果所求變量 $\boldsymbol{s}$ 在當(dāng)前時(shí)刻 $n$ 的值 $\boldsymbol{s}[n]$ 和前一時(shí)刻的值 $\boldsymbol{s}[n - 1]$ 存在線性關(guān)系锯七，即
$\boldsymbol{s}[n] = \boldsymbol{A}[n - 1]\boldsymbol{s}[n - 1]+\boldsymbol{B}[n - 1]\boldsymbol{u}[n]$

其中驅(qū)動(dòng)噪聲 $\boldsymbol{u}[n] \backsim \mathcal{N}(\boldsymbol{0},\boldsymbol{Q})$ 链快，該方程俗稱狀態(tài)方程，表明了變量自身隨著時(shí)間的變化情況眉尸，該方程一般和物理學(xué)域蜗、運(yùn)動(dòng)學(xué)等其他學(xué)科相關(guān)

同時(shí)對(duì)于所求變量 $\boldsymbol{s}$ 會(huì)有相應(yīng)的觀測(cè) $\boldsymbol{x}$ ，在線性假設(shè)下噪猾，認(rèn)為有
$\boldsymbol{x}[n] = \boldsymbol{H}[n] \boldsymbol{s}[n] + \boldsymbol{w}[n]$

其中觀測(cè)噪聲 $\boldsymbol{w}[n] \backsim \mathcal{N}(\boldsymbol{0}, \boldsymbol{C}[n])$

令 $\boldsymbol{X}[n] = \begin{bmatrix} \boldsymbol{x}[0] \\ \vdots\\ \boldsymbol{x}[n] \end{bmatrix}$ 霉祸，在線性假設(shè)下，有 $\hat{\boldsymbol{s}}[n] = \boldsymbol{K} \boldsymbol{X}[n]+\boldsymbol{t}$ 袱蜡，根據(jù)前面LMMSE的結(jié)果可知
$\hat{\boldsymbol{s}}[n] = E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}}\boldsymbol{C}_{\boldsymbol{X}\boldsymbol{X}}^{-1} (\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))$

其中
$\begin{aligned} \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}} =& E((\boldsymbol{s}[i] - E(\boldsymbol{s}[i]))(\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))^T) \\ \boldsymbol{C}_{\boldsymbol{X}\boldsymbol{X}} =& E((\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))(\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))^T) \end{aligned}$

上式中 $\hat{\boldsymbol{s}}[n]$ 定義為 $\boldsymbol{X}[n]$ 的線性函數(shù)丝蹭，結(jié)果記為 $\hat{\boldsymbol{s}}[n|n]$ ，可以看到每次計(jì)算都需要聯(lián)合 $\boldsymbol{x}[0], \cdots, \boldsymbol{x}[n]$ 的所有數(shù)據(jù)戒劫，考慮到在時(shí)序上有很多重復(fù)計(jì)算半夷，是否可以利用前一幀的結(jié)果 $\hat{\boldsymbol{s}}[n-1|n-1]$ 和當(dāng)前的觀測(cè) $\boldsymbol{x}[n]$ 計(jì)算得到 $\hat{\boldsymbol{s}}[n|n]$ ，即序貫LMMSE

為了參數(shù)分離迅细，首先將 $\boldsymbol{X}[n]$ 分為不相關(guān)的 $\boldsymbol{X}[n-1]$ 和新息 $\tilde{\boldsymbol{x}}[n]$ 巫橄，這可以通過(guò) Gram-Schmidt 正交化來(lái)實(shí)現(xiàn)，如下式所示
$\tilde{\boldsymbol{x}}[n] = \boldsymbol{x}[n] - \hat{\boldsymbol{x}}[n|n-1]$

其中 $\hat{\boldsymbol{x}}[n|n-1]$ 是將 $\boldsymbol{x}[n]$ 看做估計(jì)量 $\boldsymbol{X}[n-1]$ 看做觀測(cè)量茵典，在LMMSE下的結(jié)果湘换，如下所示
$\hat{\boldsymbol{x}}[n|n-1] = E(\boldsymbol{x}[n]) + \boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} (\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))$

可以證明 $E(\tilde{\boldsymbol{x}}[n]\boldsymbol{X}^T[n-1]) = E(\tilde{\boldsymbol{x}}[n])E(\boldsymbol{X}^T[n-1])$ ，過(guò)程如下
左邊等于
$\begin{aligned} &E(\tilde{\boldsymbol{x}}[n]\boldsymbol{X}^T[n-1]) \\ =& E(\boldsymbol{x}[n]\boldsymbol{X}^T[n-1]) - E(\hat{\boldsymbol{x}}[n|n-1]\boldsymbol{X}^T[n-1]) \\ =& E(\boldsymbol{x}[n]\boldsymbol{X}^T[n-1]) - E(\boldsymbol{x}[n])E(\boldsymbol{X}^T[n-1]) \\ &-\boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]} \\ =& E(\boldsymbol{x}[n]\boldsymbol{X}^T[n-1]) - E(\boldsymbol{x}[n])E(\boldsymbol{X}^T[n-1]) - \boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]} \\ =& \boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]} - \boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]} \\ =& 0 \end{aligned}$

右邊等于
$E(\tilde{\boldsymbol{x}}[n]) = E(\boldsymbol{x}[n]) - E(\hat{\boldsymbol{x}}[n|n-1]) = 0$

將 $\boldsymbol{X}[n-1], \tilde{\boldsymbol{x}}[n]$ 組成新的向量 $\boldsymbol{X}'[n] = \begin{bmatrix} \boldsymbol{X}[n-1]\\ \tilde{\boldsymbol{x}}[n] \end{bmatrix}$ 统阿，注意到 $\boldsymbol{X}[n]$ 可以由 $\boldsymbol{X}'[n]$ 線性變換得到彩倚，所以兩者的 LMMSE 結(jié)果一致，將前面得到的關(guān)于 $\boldsymbol{X}[n]$ 的 LMMSE 結(jié)果轉(zhuǎn)變成關(guān)于 $\boldsymbol{X}'[n]$ 的 LMMSE 結(jié)果扶平，如下所示
$\hat{\boldsymbol{s}}[n|n] = E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}'}\boldsymbol{C}_{\boldsymbol{X}'\boldsymbol{X}'}^{-1} (\boldsymbol{X}'[n] - E(\boldsymbol{X}'[n]))$

其中
$\begin{aligned} \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}'} = \begin{bmatrix} \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}[n-1]} & \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]} \end{bmatrix} \\ \boldsymbol{C}_{\boldsymbol{X}'\boldsymbol{X}'} = \begin{bmatrix} \boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]} & \boldsymbol{0} \\ \boldsymbol{0} &\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]} \end{bmatrix} \end{aligned}$

注意到矩陣中存在部分0帆离，因此轉(zhuǎn)變后的 LMMSE 結(jié)果可以拆成
$\begin{aligned} \hat{\boldsymbol{s}}[n|n] =& E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}[n]\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} (\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))\\ & + \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]}^{-1} (\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n])) \end{aligned}$

利用 $\boldsymbol{s}[n] - E(\boldsymbol{s}[n]) = \boldsymbol{A}[n - 1]\boldsymbol{s}[n - 1] - \boldsymbol{A}[n - 1]E(\boldsymbol{s}[n - 1])+\boldsymbol{B}[n - 1]\boldsymbol{u}[n]$ 可以得到
$\begin{aligned} \boldsymbol{C}_{\boldsymbol{s}[n]\boldsymbol{X}[n-1]} =& E((\boldsymbol{s}[n] - E(\boldsymbol{s}[n]))(\boldsymbol{X}[n - 1] - E(\boldsymbol{X}[n - 1]))^T)\\ =& \boldsymbol{A}[n - 1]\boldsymbol{C}_{\boldsymbol{s}[n-1]\boldsymbol{X}[n-1]} \end{aligned}$

繼而可以得到
$\begin{aligned} &\hat{\boldsymbol{s}}[n|n]\\ =& \boldsymbol{A}[n - 1]E(\boldsymbol{s}[n - 1]) \\ &+ \boldsymbol{A}[n - 1]\boldsymbol{C}_{\boldsymbol{s}[n-1]\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} (\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))\\ & + \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]}^{-1} (\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n])) \\ =& \boldsymbol{A}[n - 1] \hat{\boldsymbol{s}}[n-1|n-1] + \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]}^{-1} (\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n])) \end{aligned}$

如果令 $\hat{\boldsymbol{s}}[n|n-1] = \boldsymbol{A}[n - 1] \hat{\boldsymbol{s}}[n-1|n-1]$ ，則有
$\begin{aligned} \hat{\boldsymbol{s}}[n|n] =& \hat{\boldsymbol{s}}[n|n-1] + \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]}^{-1} (\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n])) \end{aligned}$

其中
$\begin{aligned} \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]} =& E((\boldsymbol{s}[n] - E(\boldsymbol{s}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \\ \boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]} =& E((\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \end{aligned}$

根據(jù) $\hat{\boldsymbol{x}}[n|n - 1]$ 的表達(dá)式结澄，很容易證明
$\hat{\boldsymbol{x}}[n|n - 1] = \boldsymbol{H}[n]\hat{\boldsymbol{s}}[n|n - 1]$

因此有
$\begin{aligned} \tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]) =& \tilde{\boldsymbol{x}}[n] \\ =& \boldsymbol{x}[n] - \hat{\boldsymbol{x}}[n|n-1] \\ =& \boldsymbol{x}[n] - \boldsymbol{H}[n]\hat{\boldsymbol{s}}[n|n - 1] \\ =& \boldsymbol{H}[n]\boldsymbol{s}[n] + \boldsymbol{w}[n] - \boldsymbol{H}[n]\hat{\boldsymbol{s}}[n|n - 1] \\ =& \boldsymbol{H}[n](\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1]) + \boldsymbol{w}[n] \end{aligned}$

利用 $\hat{\boldsymbol{s}}[n|n-1] = E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} (\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))$ 和 $E((\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) = 0$ 哥谷，可以得到
$\begin{aligned} &\boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\\ =& E((\boldsymbol{s}[n] - E(\boldsymbol{s}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \\ =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \\ =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{H}[n](\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1]) + \boldsymbol{w}[n])^T) \\ =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])^T) \boldsymbol{H}^T[n] \end{aligned}$

令 $\boldsymbol{M}[n|n - 1] = E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])^T)$ ，則上式變成
$\boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]} = \boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n]$

另一方面利用 $\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]) = \boldsymbol{H}[n](\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1]) + \boldsymbol{w}[n]$ 麻献，可以得到
$\begin{aligned} \boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]} =& E((\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \\ =& \boldsymbol{H}[n] \boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n] + \boldsymbol{C}[n] \end{aligned}$

因此有
$\hat{\boldsymbol{s}}[n|n] = \hat{\boldsymbol{s}}[n|n-1] + \boldsymbol{K}[n](\boldsymbol{x}[n] - \boldsymbol{H}[n]\hat{\boldsymbol{s}}[n|n - 1])$

其中
$\begin{aligned} \boldsymbol{K}[n] =& \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} \\ =& \boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n](\boldsymbol{H}[n] \boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n] + \boldsymbol{C}[n])^{-1} \end{aligned}$

最后需要給出 $\boldsymbol{M}[n|n - 1]$ 和 $\boldsymbol{M}[n|n]$ 的遞推公式
利用 $\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1] = \boldsymbol{A}[n - 1]\boldsymbol{s}[n-1] + \boldsymbol{B}[n - 1]\boldsymbol{u[n]} - \boldsymbol{A}[n - 1] \hat{\boldsymbol{s}}[n - 1|n - 1]$ 们妥，可以得到
$\begin{aligned} \boldsymbol{M}[n|n - 1] =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])^T) \\ =& \boldsymbol{A}[n - 1]\boldsymbol{M}[n-1|n-1]\boldsymbol{A}^T[n - 1] + \boldsymbol{B}[n - 1]\boldsymbol{Q}\boldsymbol{B}^T[n - 1] \end{aligned}$

利用 $\hat{\boldsymbol{s}}[n|n] = \hat{\boldsymbol{s}}[n|n-1] + \boldsymbol{K}[n](\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))$ 可以得到
$\begin{aligned} \boldsymbol{M}[n|n] =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n])^T) \\ =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])^T)\\ &- 2E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \boldsymbol{K}^T[n]\\ &+\boldsymbol{K}[n] E(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \boldsymbol{K}^T[n]\\ =& \boldsymbol{M}[n|n-1] \\ &- 2 \boldsymbol{M}[n|n-1] \boldsymbol{H}^T[n]\boldsymbol{K}^T[n]\\ &+ \boldsymbol{M}[n|n-1] \boldsymbol{H}^T[n]\boldsymbol{K}^T[n] \\ =& (\boldsymbol{I} - \boldsymbol{K}[n]\boldsymbol{H}[n])\boldsymbol{M}[n|n - 1] \end{aligned}$

至此所有遞推公式都推導(dǎo)完畢，可以很清楚地看到一切皆起源于方程
$\hat{\boldsymbol{s}}[n] = E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}}\boldsymbol{C}_{\boldsymbol{X}\boldsymbol{X}}^{-1} (\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))$

后面的推導(dǎo)只是在完成該方程序貫形式下的結(jié)果罷了勉吻，而該方程的起源來(lái)自于線性假設(shè)监婶，所以卡爾曼濾波本質(zhì)上是非平穩(wěn)信號(hào)下的序貫線性最小均方誤差估計(jì)量，其最終結(jié)果可以總結(jié)如下步驟：
初始化：
$\hat{\boldsymbol{s}}[-1|-1] = \boldsymbol{\mu}_{\boldsymbol{s}}, \boldsymbol{M}[-1|-1] = \boldsymbol{C}_{\boldsymbol{s}}$

預(yù)測(cè)估計(jì)量：
$\hat{\boldsymbol{s}}[n|n - 1] = \boldsymbol{A}[n - 1]\hat{\boldsymbol{s}}[n - 1|n - 1]$

預(yù)測(cè)最小 MSE 矩陣：
$\boldsymbol{M}[n|n - 1] = \boldsymbol{A}[n - 1] \boldsymbol{M}[n - 1| n - 1] \boldsymbol{A}^T[n - 1] + \boldsymbol{B}[n - 1]\boldsymbol{Q}\boldsymbol{B}^T[n - 1]$

增益矩陣：
$\boldsymbol{K}[n] = \boldsymbol{M}[n|n - 1] \boldsymbol{H}^T[n](\boldsymbol{H}[n]\boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n] + \boldsymbol{C}[n])^{-1}$

修正估計(jì)量：
$\hat{\boldsymbol{s}}[n|n] = \hat{\boldsymbol{s}}[n|n - 1] + \boldsymbol{K}[n](\boldsymbol{x}[n] - \boldsymbol{H}[n] \hat{\boldsymbol{s}}[n|n - 1])$

修正最小 MSE 矩陣：
$\boldsymbol{M}[n|n] = (\boldsymbol{I} - \boldsymbol{K}[n] \boldsymbol{H}[n]) \boldsymbol{M}[n|n - 1]$

4齿桃、擴(kuò)展卡爾曼濾波

卡爾曼濾波基于線性假設(shè)惑惶，當(dāng)狀態(tài)方程和觀測(cè)方程均不為線性的時(shí)候，如下所示
$\begin{aligned} \boldsymbol{s}[n] =& a(\boldsymbol{s}[n - 1]) + \boldsymbol{B}[n - 1]\boldsymbol{u}[n] \\ \boldsymbol{x}[n] =& h(\boldsymbol{s}[n]) + \boldsymbol{w}[n] \end{aligned}$

如果認(rèn)為前一時(shí)刻的值幾乎在真值附近源譬，將非線性函數(shù) $a,h$ 進(jìn)行泰勒展開(kāi)可以得到
$\begin{aligned} a(\boldsymbol{s}[n - 1]) \approx& a(\tilde{\boldsymbol{s}}[n - 1 | n - 1]) + \boldsymbol{A}[n - 1](\boldsymbol{s}[n - 1] - \tilde{\boldsymbol{s}}[n - 1 | n - 1]) \\ h(\boldsymbol{s}[n]) \approx& h(\tilde{\boldsymbol{s}}[n| n - 1]) + \boldsymbol{H}[n](\boldsymbol{s}[n] - \tilde{\boldsymbol{s}}[n | n - 1]) \\ \end{aligned}$

其中 $\boldsymbol{A}[n - 1] = \frac{\partial a(\boldsymbol{x})}{\partial \boldsymbol{x}}\bigg|_{\boldsymbol{x} = \tilde{\boldsymbol{s}}[n - 1 | n - 1]}, \boldsymbol{H}[n] = \frac{\partial h(\boldsymbol{x})}{\partial \boldsymbol{x}} \bigg|_{\boldsymbol{x} = \tilde{\boldsymbol{s}}[n | n - 1]}$

則原狀態(tài)和觀測(cè)方程將會(huì)變?yōu)?br> $\begin{aligned} \boldsymbol{s}[n] =& \boldsymbol{A}[n - 1] \boldsymbol{s}[n - 1] + \boldsymbol{B}[n - 1]\boldsymbol{u}[n] + \boldsymbol{a}[n - 1] \\ \boldsymbol{x}[n] =& \boldsymbol{H}[n] \boldsymbol{s}[n] + \boldsymbol{w}[n] + \boldsymbol{h}[n - 1] \end{aligned}$

其中 $\boldsymbol{a}[n - 1] = a(\tilde{\boldsymbol{s}}[n - 1 | n - 1]) - \boldsymbol{A}[n - 1] \tilde{\boldsymbol{s}}[n - 1 | n - 1]$ 集惋， $\boldsymbol{h}[n - 1] = h(\tilde{\boldsymbol{s}}[n| n - 1]) - \boldsymbol{H}[n] \tilde{\boldsymbol{s}}[n | n - 1]$

最終結(jié)果可以總結(jié)如下步驟：
初始化：
$\hat{\boldsymbol{s}}[-1|-1] = \boldsymbol{\mu}_{\boldsymbol{s}}, \boldsymbol{M}[-1|-1] = \boldsymbol{C}_{\boldsymbol{s}}$

預(yù)測(cè)估計(jì)量：
$\hat{\boldsymbol{s}}[n|n - 1] = a(\hat{\boldsymbol{s}}[n - 1|n - 1])$

增益矩陣：
$\boldsymbol{K}[n] = \boldsymbol{M}[n|n - 1] \boldsymbol{H}^T[n](\boldsymbol{H}[n]\boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n] + \boldsymbol{C}[n])^{-1}$

修正估計(jì)量：
$\hat{\boldsymbol{s}}[n|n] = \hat{\boldsymbol{s}}[n|n - 1] + \boldsymbol{K}[n](\boldsymbol{x}[n] - h(\hat{\boldsymbol{s}}[n|n - 1]))$

修正最小 MSE 矩陣：
$\boldsymbol{M}[n|n] = (\boldsymbol{I} - \boldsymbol{K}[n] \boldsymbol{H}[n]) \boldsymbol{M}[n|n - 1]$

可以看到與前面有差別的部分主要在于 “預(yù)測(cè)估計(jì)量” 和 “修正估計(jì)量” 兩個(gè)步驟

最后編輯于：2023.06.05 09:56:50

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者

人面猴
序言：七十年代末，一起剝皮案震驚了整個(gè)濱河市踩娘，隨后出現(xiàn)的幾起案子刮刑，更是在濱河造成了極大的恐慌，老刑警劉巖养渴，帶你破解...
沈念sama閱讀 216,402評(píng)論 6贊 499
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件雷绢，死亡現(xiàn)場(chǎng)離奇詭異，居然都是意外死亡理卑，警方通過(guò)查閱死者的電腦和手機(jī)翘紊，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 92,377評(píng)論 3贊 392
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門，熙熙樓的掌柜王于貴愁眉苦臉地迎上來(lái)藐唠，“玉大人帆疟，你說(shuō)我怎么就攤上這事鹉究。” “怎么了踪宠？”我有些...
開(kāi)封第一講書(shū)人閱讀 162,483評(píng)論 0贊 353
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵自赔，是天一觀的道長(zhǎng)。經(jīng)常有香客問(wèn)我柳琢，道長(zhǎng)绍妨，這世上最難降的妖魔是什么？我笑而不...
開(kāi)封第一講書(shū)人閱讀 58,165評(píng)論 1贊 292
?港島之戀（遺憾婚禮）
正文為了忘掉前任柬脸，我火速辦了婚禮他去，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘倒堕。我一直安慰自己灾测，他們只是感情好，可當(dāng)我...
茶點(diǎn)故事閱讀 67,176評(píng)論 6贊 388
惡毒庶女頂嫁案：這布局不是一般人想出來(lái)的
文/花漫我一把揭開(kāi)白布垦巴。她就那樣靜靜地躺著行施，像睡著了一般。火紅的嫁衣襯著肌膚如雪魂那。梳的紋絲不亂的頭發(fā)上蛾号，一...
開(kāi)封第一講書(shū)人閱讀 51,146評(píng)論 1贊 297
城市分裂傳說(shuō)
那天，我揣著相機(jī)與錄音涯雅，去河邊找鬼鲜结。笑死，一個(gè)胖子當(dāng)著我的面吹牛活逆，可吹牛的內(nèi)容都是我干的精刷。我是一名探鬼主播，決...
沈念sama閱讀 40,032評(píng)論 3贊 417
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開(kāi)眼蔗候，長(zhǎng)吁一口氣：“原來(lái)是場(chǎng)噩夢(mèng)啊……” “哼怒允！你這毒婦竟也來(lái)了？” 一聲冷哼從身側(cè)響起锈遥，我...
開(kāi)封第一講書(shū)人閱讀 38,896評(píng)論 0贊 274
萬(wàn)榮殺人案實(shí)錄
序言：老撾萬(wàn)榮一對(duì)情侶失蹤纫事，失蹤者是張志新（化名）和其女友劉穎，沒(méi)想到半個(gè)月后所灸，有當(dāng)?shù)厝嗽跇?shù)林里發(fā)現(xiàn)了一具尸體丽惶，經(jīng)...
沈念sama閱讀 45,311評(píng)論 1贊 310
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 37,536評(píng)論 2贊 332
?白月光啟示錄
正文我和宋清朗相戀三年爬立，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了钾唬。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片。...
茶點(diǎn)故事閱讀 39,696評(píng)論 1贊 348
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡，死狀恐怖抡秆，靈堂內(nèi)的尸體忽然破棺而出奕巍，到底是詐尸還是另有隱情，我是刑警寧澤儒士，帶...
沈念sama閱讀 35,413評(píng)論 5贊 343
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布伍绳，位于F島的核電站，受9級(jí)特大地震影響乍桂，放射性物質(zhì)發(fā)生泄漏。R本人自食惡果不足惜效床，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 41,008評(píng)論 3贊 325
男人毒藥：我在死后第九天來(lái)索命
文/蒙蒙一睹酌、第九天我趴在偏房一處隱蔽的房頂上張望。院中可真熱鬧剩檀，春花似錦憋沿、人聲如沸。這莊子的主人今日做“春日...
開(kāi)封第一講書(shū)人閱讀 31,659評(píng)論 0贊 22
一樁弒父案辐啄，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽(yáng)。三九已至运嗜，卻和暖如春壶辜，著一層夾襖步出監(jiān)牢的瞬間，已是汗流浹背担租。一陣腳步聲響...
開(kāi)封第一講書(shū)人閱讀 32,815評(píng)論 1贊 269
情欲美人皮
我被黑心中介騙來(lái)泰國(guó)打工砸民，沒(méi)想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留，地道東北人奋救。一個(gè)月前我還...
沈念sama閱讀 47,698評(píng)論 2贊 368
代替公主和親
正文我出身青樓岭参，卻偏偏與公主長(zhǎng)得像，于是被迫代替她去往敵國(guó)和親尝艘。傳聞我的和親對(duì)象是個(gè)殘疾皇子演侯，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 44,592評(píng)論 2贊 353

卡爾曼濾波

1贮缕、最小均方誤差

2撕贞、線性最小均方誤差

3、卡爾曼濾波

4齿桃、擴(kuò)展卡爾曼濾波

推薦閱讀更多精彩內(nèi)容