卡爾曼濾波

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

1、最小均方誤差

對(duì)于經(jīng)典的均方誤差（Mean Square Error, MSE）方法認(rèn)為，被估計(jì)量 $\boldsymbol{\theta}$ 是一個(gè)確定的未知常量，因此經(jīng)典MSE通過求解下式最小得到估計(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é)果并沒有考慮估計(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})$ 來求解的，使得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可以寫成
$\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]$ ，這可以通過 Gram-Schmidt 正交化來實(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])$ ，過程如下
左邊等于
$\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é)果罷了，而該方程的起源來自于線性假設(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)行泰勒展開可以得到
$\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)系作者
平臺(tái)聲明：文章內(nèi)容（如有圖片或視頻亦包括在內(nèi)）由作者上傳并發(fā)布，文章內(nèi)容僅代表作者本人觀點(diǎn)，簡(jiǎn)書系信息發(fā)布平臺(tái)，僅提供信息存儲(chǔ)服務(wù)。

人面猴
序言：七十年代末，一起剝皮案震驚了整個(gè)濱河市，隨后出現(xiàn)的幾起案子，更是在濱河造成了極大的恐慌，老刑警劉巖，帶你破解...
沈念sama閱讀 230,563評(píng)論 6贊 544
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件，死亡現(xiàn)場(chǎng)離奇詭異，居然都是意外死亡，警方通過查閱死者的電腦和手機(jī)，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 99,694評(píng)論 3贊 429
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門，熙熙樓的掌柜王于貴愁眉苦臉地迎上來，“玉大人，你說我怎么就攤上這事。” “怎么了？”我有些...
開封第一講書人閱讀 178,672評(píng)論 0贊 383
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵，是天一觀的道長(zhǎng)。經(jīng)常有香客問我，道長(zhǎng)，這世上最難降的妖魔是什么？我笑而不...
開封第一講書人閱讀 63,965評(píng)論 1贊 318
?港島之戀（遺憾婚禮）
正文為了忘掉前任，我火速辦了婚禮，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘。我一直安慰自己，他們只是感情好，可當(dāng)我...
茶點(diǎn)故事閱讀 72,690評(píng)論 6贊 413
惡毒庶女頂嫁案：這布局不是一般人想出來的
文/花漫我一把揭開白布。她就那樣靜靜地躺著，像睡著了一般。火紅的嫁衣襯著肌膚如雪。梳的紋絲不亂的頭發(fā)上，一...
開封第一講書人閱讀 56,019評(píng)論 1贊 329
城市分裂傳說
那天，我揣著相機(jī)與錄音，去河邊找鬼。笑死，一個(gè)胖子當(dāng)著我的面吹牛，可吹牛的內(nèi)容都是我干的。我是一名探鬼主播，決...
沈念sama閱讀 44,013評(píng)論 3贊 449
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開眼，長(zhǎng)吁一口氣：“原來是場(chǎng)噩夢(mèng)啊……” “哼！你這毒婦竟也來了？” 一聲冷哼從身側(cè)響起，我...
開封第一講書人閱讀 43,188評(píng)論 0贊 290
萬榮殺人案實(shí)錄
序言：老撾萬榮一對(duì)情侶失蹤，失蹤者是張志新（化名）和其女友劉穎，沒想到半個(gè)月后，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體，經(jīng)...
沈念sama閱讀 49,718評(píng)論 1贊 336
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 41,438評(píng)論 3贊 360
?白月光啟示錄
正文我和宋清朗相戀三年，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片。...
茶點(diǎn)故事閱讀 43,667評(píng)論 1贊 374
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡，死狀恐怖，靈堂內(nèi)的尸體忽然破棺而出，到底是詐尸還是另有隱情，我是刑警寧澤，帶...
沈念sama閱讀 39,149評(píng)論 5贊 365
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布，位于F島的核電站，受9級(jí)特大地震影響，放射性物質(zhì)發(fā)生泄漏。R本人自食惡果不足惜，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 44,845評(píng)論 3贊 351
男人毒藥：我在死后第九天來索命
文/蒙蒙一、第九天我趴在偏房一處隱蔽的房頂上張望。院中可真熱鬧，春花似錦、人聲如沸。這莊子的主人今日做“春日...
開封第一講書人閱讀 35,252評(píng)論 0贊 28
一樁弒父案，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽。三九已至，卻和暖如春，著一層夾襖步出監(jiān)牢的瞬間，已是汗流浹背。一陣腳步聲響...
開封第一講書人閱讀 36,590評(píng)論 1贊 295
情欲美人皮
我被黑心中介騙來泰國(guó)打工，沒想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留，地道東北人。一個(gè)月前我還...
沈念sama閱讀 52,384評(píng)論 3贊 400
代替公主和親
正文我出身青樓，卻偏偏與公主長(zhǎng)得像，于是被迫代替她去往敵國(guó)和親。傳聞我的和親對(duì)象是個(gè)殘疾皇子，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 48,635評(píng)論 2贊 380

三个男躁一个女,国精产品一区一手机的秘密,麦子交换系列最经典十句话,欧美国产综合欧美视频

卡爾曼濾波

卡爾曼濾波

1、最小均方誤差

2、線性最小均方誤差

3、卡爾曼濾波

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

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

三个男躁一个女,国精产品一区一手机的秘密,麦子交换系列最经典十句话,欧美 国产 综合 欧美 视频

卡爾曼濾波

1、最小均方誤差

2、線性最小均方誤差

3、卡爾曼濾波

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

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

三个男躁一个女,国精产品一区一手机的秘密,麦子交换系列最经典十句话,欧美国产综合欧美视频