【強基固本】卡爾曼濾波器

taotao_2016 2020-12-04

展開全文

轉(zhuǎn)載來源：https://longaspire./blog/卡爾曼濾波/

Kalman Filter (KF) 是一個高效的遞歸濾波器,，它可以實現(xiàn)從一系列的噪聲觀測中，估計動態(tài)系統(tǒng)的狀態(tài),。

命名和發(fā)展歷史

卡爾曼濾波器以它的發(fā)明者Rudolf. Emil. Kalman先生（2016年去世,，向這位傳奇的科學家致敬）而命名。

在控制領(lǐng)域,，Kalman濾波被稱為線性二次型估計,。其也可以認為是一個最優(yōu)化自回歸數(shù)據(jù)處理算法（optimal recursive data processing algorithm）。

目前,，卡爾曼濾波已經(jīng)有很多不同的實現(xiàn),，有施密特擴展濾波器、信息濾波器以及一系列的Bierman和Thornton發(fā)明的平方根濾波器等,，而卡爾曼最初提出的形式現(xiàn)在稱為簡單卡爾曼濾波器,。也許最常見的卡爾曼濾波器應用是鎖相環(huán)，它在收音機,、計算機和幾乎全部視頻或通訊設備中廣泛存在,。

一個簡單的應用是估計物體的位置和速度。簡要描述如下：假設我們可以獲取一個物體的包含噪聲的一系列位置觀測數(shù)據(jù),，我們可以獲得此物體的精確速度和位置連續(xù)更新信息,。例如，對于雷達來說,，我們關(guān)心的是跟蹤目標,，而目標的位置、速度,、加速度的觀測值是時刻含有誤差的,，卡爾曼濾波器利用目標的動態(tài)信息，去掉噪聲影響,，獲取目標此刻好的位置估計（即濾波過程）,，將來的位置估計（即預測過程），也可以是過去位置估計的（即插值或平滑過程）,。

應用舉例

假設我們要研究的對象是一個房間的溫度,。根據(jù)你的經(jīng)驗判斷，這個房間的溫度是恒定的,，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設我們用一分鐘來做時間單位）,。

假設你對你的經(jīng)驗不是絕對相信，可能會有上下偏差幾度,。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise,，理想情況下我們以高斯噪聲來進行假設估計），也就是這些偏差跟前后時間是沒有關(guān)系的而且符合高斯分布（Gaussian Distribution）,。

另外,，我們在房間里放一個溫度計,，但是這個溫度計也不準確的，觀測值會比實際值偏差,。我們也把這些偏差看成是高斯白噪聲,。好了，現(xiàn)在對于某一分鐘我們有兩個有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗的預測值（系統(tǒng)的預測值）和溫度計的值（觀測值）,。下面我們要用這兩個值結(jié)合它們各自的噪聲來估算出房間的實際溫度值,。

假如我們要估算

k

時刻的實際溫度值。首先你要根據(jù)

k-1

時刻的溫度值,，來預測

k

時刻的溫度,。因為你相信溫度是恒定的，所以你會得到

k

時刻的溫度預測值是跟

k-1

時刻一樣的,，假設是23度,，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果

k-1

時刻估算出的最優(yōu)溫度值的偏差是3，你對自己預測的不確定度是4度,，它們平方相加再開方就是5）,。

然后，你從溫度計那里得到了

k

時刻的溫度值,，假設是25度,，同時該值的偏差是4度。由于我們用于估算

k

時刻的實際溫度有兩個溫度值,，分別是23度和25度,。究竟實際溫度是多少呢？相信自己還是相信溫度計呢,？究竟相信誰多一點,，我們可以用它們的協(xié)方差（covariance）來判斷。

因為

\mathrm{Kg}^{2}=5^{2} /\left(5^{2}+4^{2}\right)

,，所以

\mathrm{Kg}=0.78

，我們可以估算出

k

時刻的實際溫度值是：

23+0.78 *(25-23)=24.56

度,。

可以看出,，因為溫度計的協(xié)方差比較小（比較相信溫度計）,，所以估算出的最優(yōu)溫度值偏向溫度計的值,。

現(xiàn)在我們已經(jīng)得到

k

時刻的最優(yōu)溫度值了，下一步就是要進入

k+1

時刻,，進行新的最優(yōu)估算,。在進入

k+1

時刻之前，我們還要算出

k

時刻那個最優(yōu)值（24.56度）的偏差,。算法如下：

\left((1-\mathrm{Kg}) * 5^{2}\right)^{0.5}=2.35

,。

這里的5就是上面的

k

時刻你預測的那個23度溫度值的偏差,，得出的2.35就是進入

k+1

時刻以后

k

時刻估算出的最優(yōu)溫度值的偏差（對應于上面的3）。就是這樣,，卡爾曼濾波器就不斷的把協(xié)方差遞歸,，從而估算出最優(yōu)的溫度值。它運行的很快,，而且它只保留了上一時刻的協(xié)方差,。

上面的 $\mathrm{Kg}$ ，就是卡爾曼增益（Kalman Gain）,，可以隨不同的時刻而改變自己的值,，后續(xù)會進一步說明其計算方法和數(shù)學意義。

卡爾曼濾波器算法

卡爾曼濾波基于時域描述的線性動態(tài)系統(tǒng),，它的模型是馬爾科夫鏈（Markov Chain）,，而馬爾科夫鏈建立在一個被高斯噪聲干擾的線性算子之上。

系統(tǒng)的狀態(tài)可以用一個元素為實數(shù)的向量表示,。隨著離散時間的增加,，這個線性算子就會作用到當前狀態(tài)之上，產(chǎn)生一個新的狀態(tài),，并且會帶入一定的噪聲,，同時一些已知的控制信息也會加入。同時,，另外一個受噪聲干擾的線性算子將產(chǎn)生這些隱含狀態(tài)的可見輸出,。

卡爾曼濾波器可以被看作為類隱馬爾科夫模型，它們的顯著不同點在于：

隱狀態(tài)變量的取值空間是一個連續(xù)的空間,，而不是離散的狀態(tài)空間,；
另外，隱馬爾科夫模型可以描述下一個狀態(tài)的一個任意分布,，這也與應用于卡爾曼濾波器中的高斯噪聲模型相反,。

先看一下動態(tài)系統(tǒng)的基本模型。

動態(tài)模型

首先,，我們先要引入一個離散控制過程的系統(tǒng),。該系統(tǒng)的過程模型可用一個線性隨機微分方程（Linear Stochastic Difference Equation）來描述：

x(k)=\mathbf{F} \cdot x(k-1)+\mathbf{B} \cdot u(k)+w(k)

再加上系統(tǒng)觀測模型：

z(k)=\mathbf{H} \cdot x(k)+v(k)

上兩式子中，

x(k)

是

k

時刻的系統(tǒng)狀態(tài),，

u(k)

是

k

時刻對系統(tǒng)的控制量,。

\mathbf{F}

和

\mathbf{B}

是系統(tǒng)參數(shù)，對于多模型系統(tǒng),，它們?yōu)檗D(zhuǎn)移矩陣,。

z(k)

是

k

時刻的觀測值，

\mathbf{H}

是觀測系統(tǒng)的參數(shù)，對于多觀測系統(tǒng),，

\mathbf{H}

為矩陣,。

w(k)

和

v(k)

分別表示過程和觀測的噪聲。它們被假設成高斯白噪聲（White Gaussian Noise）,，它們的協(xié)方差分別是

\mathbf{Q}

,，

\mathbf{R}

（這里我們假設它們不隨系統(tǒng)狀態(tài)變化而變化）。

卡爾曼濾波是一種遞歸的估計,，即只要獲知上一時刻狀態(tài)的估計值以及當前狀態(tài)的觀測值就可以計算出當前狀態(tài)的估計值,，因此不需要記錄觀測或者估計的歷史信息。

卡爾曼濾波器與大多數(shù)濾波器不同之處在于：它是一種純粹的時域濾波器,，它不需要像低通濾波器等頻域濾波器那樣,，需要在頻域設計再轉(zhuǎn)換到時域?qū)崿F(xiàn)。

卡爾曼濾波器的操作包括兩個階段：預測與更新：

在預測階段,，濾波器使用上一狀態(tài)的估計,，做出對當前狀態(tài)的估計。
在更新階段,，濾波器利用對當前狀態(tài)的觀測值優(yōu)化在預測階段獲得的預測值,，以獲得一個更精確的新估計值。

3.1 預測階段

對于滿足上面的條件（線性隨機微分系統(tǒng),，過程和觀測都是高斯白噪聲）,，卡爾曼濾波器是最優(yōu)的信息處理器。

下面我們來用它們結(jié)合它們的協(xié)方差來估算系統(tǒng)的最優(yōu)化輸出（類似上一節(jié)那個溫度的例子）,。首先,，我們要利用系統(tǒng)的過程模型，來預測下一狀態(tài)的系統(tǒng),。假設現(xiàn)在的系統(tǒng)狀態(tài)是

k

,，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預測出現(xiàn)在狀態(tài)：

x(k \mid k-1) = \mathbf{F} \cdot x(k-1 \mid k-1) + \mathbf{B} \cdot u(k)

上述公式稱為預測的狀態(tài)估計方程,，其中,，

x(k \mid k-1)

是利用上一狀態(tài)預測的結(jié)果，

x(k-1 \mid k-1)

是上一狀態(tài)最優(yōu)的結(jié)果,，

u(k)

為現(xiàn)在狀態(tài)的控制量,，如果沒有控制量，它可以為0,。

到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了,，可是,，對應于

x(k \mid k-1)

的協(xié)方差（covariance）還沒更新。我們用

P

表示協(xié)方差，它實際上描述了預測值的準確程度：

P(k \mid k-1) = \mathbf{F} \cdot P(k-1 \mid k-1) \cdot \mathbf{F}^T + \mathbf{Q}_k

上述公式稱為預測的協(xié)方差矩陣估計方程,，其中,，

P(k \mid k-1)

是

x(k \mid k-1)

對應的協(xié)方差，

P(k-1 \mid k-1)

是

x(k-1 \mid k-1)

對應的協(xié)方差,，

\mathbf{F}^T

表示

\mathbf{F}

的轉(zhuǎn)置矩陣,，是系統(tǒng)過程的協(xié)方差。

上述兩個公式就是對系統(tǒng)的預測,。

3.2 更新階段

在進行更新之前,，我們先計算三個值：

首先是觀測余量（measurement residual）：

y(k) = z(k) - \mathbf{H} \cdot x(k \mid k-1)

因為觀測過程中存在一個觀測誤差的協(xié)方差矩陣，我們可以給出一個觀測余量的協(xié)方差：

S(k) = \mathbf{H}_k \cdot P(k \mid k-1) \cdot \mathbf{H}_k^T + \mathbf{R}_k

接下來給出一個卡爾曼增益（Kalman Gain）：

\mathit{Kg}(k) = P(k \mid k-1) \cdot \mathbf{H}^T \cdot S(k)^{-1} = P(k \mid k-1) \cdot \mathbf{H}^T \cdot (\mathbf{H} \cdot P(k \mid k-1) \cdot \mathbf{H}^T + \mathbf{R}_k)^{-1}

現(xiàn)在我們有了現(xiàn)在狀態(tài)的預測結(jié)果,，然后我們再收集現(xiàn)在狀態(tài)的觀測值,。結(jié)合預測值和觀測值，我們可以得到現(xiàn)在狀態(tài)

k

的最優(yōu)化估算值

x(k \mid k)

：

x(k \mid k) = x(k \mid k-1) + \mathit{Kg}(k) \cdot y(k)

上述方程為更新的狀態(tài)估計方程,。

到現(xiàn)在為止,，我們已經(jīng)得到了

k

狀態(tài)下最優(yōu)的估算值

x(k \mid k)

。但是為了要使得卡爾曼濾波器不斷的運行下去直到系統(tǒng)過程結(jié)束,，我們還要更新k狀態(tài)下

x(k \mid k)

的covariance：

P(k \mid k) = ( \mathbf{I} - \mathit{Kg}(k) \cdot \mathbf{H}) \cdot P(k \mid k-1)

上述方程成為更新的協(xié)方差矩陣估計方程,，其中

\mathbf{I}

為單位矩陣，對于單模型單觀測,，

\mathbf{I} = 1

,。當系統(tǒng)進入k+1狀態(tài)時，

P(k \mid k)

就是預測方程中的

P(k-1 \mid k-1)

,。這樣,，算法就可以自回歸的運算下去。

卡爾曼增益的意義

以上一直提到卡爾曼增益,，那么其實際意義如何理解呢,？我們結(jié)合一些數(shù)學知識來對其進行解釋。

我們知道,，卡爾曼濾波器對于隨機變量的噪聲,，加入了高斯分布的假設，而這,，也是能夠?qū)B續(xù)動態(tài)信息進行濾波的基礎(chǔ),。

那么先從高斯分布開始：符合高斯分布的一個隨機變量

X

具有一個平均值

\mu

和一個方差

D

。平均值

\mu

即數(shù)學期望

E[X]

,，而方差

D = \sigma^2 = E[(x - E[x])^2]

,，衡量隨機變量或一組數(shù)據(jù)時離散程度的度量。

\sigma

即標準差或者均方差,。正態(tài)分布的公式：

\mathcal{N}(x, \mu, \sigma) = \frac{1}{\sigma \sqrt{2 \pi}} e^{-\frac{(x-\mu)^2}{2\sigma^2}}

而兩個隨機變量X和Y的協(xié)方差可以寫為：

\text{cov}(X, Y) = E[(X - E[X])(Y - E[Y])]= E[XY] - 2E[X]E[Y] + E[X]E[Y]= E[XY] - E[X]E[Y]

協(xié)方差衡量了隨機變量間的相關(guān)程度,。

高斯分布的一個重要性質(zhì)是：互不相關(guān)的兩個高斯分布相乘（相互疊加）后,，仍然是一個正態(tài)分布！

并且新的正態(tài)分布的均值和方差滿足：

以上是單變量概率密度函數(shù)的計算結(jié)果,，如果是多變量的,，那么，就變成了協(xié)方差矩陣 $\Sigma$ 的形式：

再結(jié)合第三部分我們結(jié)合的卡爾曼濾波器算法,，可以知道,，此處卡爾曼增益正是此處的K。它正是用來計算當前兩個高斯噪聲疊加后的系統(tǒng)情況的,。而為什么要進行疊加呢,？

我們知道，兩個事件的發(fā)生都是概率性的,，不能完全相信其中的任何一個,。如果具有兩個事件，都發(fā)生的話,，從直覺或者是理性思維上講,，兩個事件同時發(fā)生的可能性越大，我們越相信它,！要想考察它們同時發(fā)生的可能性,，就是將兩個事件單獨發(fā)生的概率相乘。但是究竟是相信自己預測還是相信觀測呢,？我們可以用卡爾曼的方法來加權(quán),，即利用他們的方差 $\sigma^2$ 來判斷——求出綠色分布均值位置在紅藍均值間的比例，即卡爾曼增益,。

紅色分布為預測,，藍色分布為觀測，綠色分布為二者的相乘

卡爾曼濾波器的理論基礎(chǔ),，正是假定觀測值和預測值的噪聲都符合正態(tài)分布,，且兩個正態(tài)分布的融合仍是正態(tài)分布這一特性進行迭代的。

結(jié)合算法模型的舉例

5.1 物體運動狀態(tài)估計

在算法模型的基礎(chǔ)上,，我們再進一步給出一個幫助理解的例子,，是從[3]的頁面上直接搬過來的：

考慮在無摩擦、無限長的直軌道上的一輛車,。該車最初停在位置0處,，但時不時受到隨機的沖擊。注意這里我們考慮沒有外力的影響,，因此忽略掉

\mathbf{B}_k

和

\mathbf{u}_k

,。同時考慮

\mathbf{F}, \mathbf{H}, \mathbf{R}, \mathbf{Q}

為常數(shù)（此處不用下標）。

我們每

\Delta t

秒即測量車的位置,，但是這個測量是非精確的,；我們想建立一個關(guān)于其位置以及速度的模型,。車的位置以及速度（或者更加一般的,，一個粒子的運動狀態(tài)）可以被線性狀態(tài)空間描述如下：

\mathbf{x_k} = \begin{bmatrix} x \\\ \dot{x} \end{bmatrix}

這其中,，

\dot{x}

是速度，即位置對時間的導數(shù),。

假設在

k-1

時刻和

k

時刻之間,，車受到

a_k

的加速度，且符合均值為0,，標準差為

\sigma_a

的正態(tài)分布,。根據(jù)牛頓第一定理，我們推出：

\mathbf{x_k} = \mathbf{F} \cdot \mathbf{x_{k-1}} + \mathbf{G} \cdot {a_k}

其中,，兩個轉(zhuǎn)移矩陣

\mathbf{F} = \begin{bmatrix} 1 & \Delta t \\\ 0 & 1 \end{bmatrix}, \mathbf{G} = \begin{bmatrix} \Delta t^2/2 \\\ {\Delta t} \end{bmatrix}

可知

\mathbf{Q} = \text{cov}(\mathbf{G} \cdot a) = E[(\mathbf{G} \cdot a) \cdot (\mathbf{G} \cdot a)^{T}] = \mathbf{G} \cdot E[a^2] \cdot \mathbf{G}^T = \mathbf{G} \cdot \sigma_a^{2} \cdot \mathbf{G}^T

每一個時刻我們都會對其進行一個測量過程,，測量受到噪聲干擾，我們依然假設噪聲服從正態(tài)分布,，均值為0,，標準差為

\sigma_z

。

z_k = \mathbf{H} \cdot x_k + v_k

其中,，我們有

\mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}, \mathbf{R} = E[v_k \cdot v_k^T] = \sigma_z^2

我們先要提出一個假設初始值,，假設車最初的位置和速度是足夠準確的：

x_{0 \mid 0} = \begin{bmatrix} 0 \\\ 0 \end{bmatrix}

并且，告訴濾波器初始的測量是準確的,，給出一個協(xié)方差矩陣：

P_{0 \mid 0} = \begin{bmatrix} 0 & 0 \\\ 0 & 0 \end{bmatrix}

如果我們不確切知道最初的位置與速度,，協(xié)方差矩陣可以初始化為一個對角線元素為B的矩陣，B取一個比較大的數(shù)

P_{0 \mid 0} = \begin{bmatrix} B & 0 \\\ 0 & B \end{bmatrix}

接下來我們就可以給出

0 + \Delta_t

時刻對于車的狀態(tài)的估計：

x_{1 \mid 0} = \begin{bmatrix} 1 & \Delta t \\\ 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} 0 \\\ 0 \end{bmatrix} + \begin{bmatrix} \Delta t^2/2 \\\ \Delta t \end{bmatrix} \cdot a_0 = \begin{bmatrix} a_0 \cdot \Delta t^2/2 \\\ \Delta t \cdot a_0 \end{bmatrix}

上個公式中的

\begin{bmatrix} a_0 \cdot \Delta t^2/2 \\\ \Delta t \cdot a_0 \end{bmatrix}

也就是

0 + \Delta_t

時刻的位移和速度的估計,。

接下來,，我們還可以得到預測的估計協(xié)方差矩陣：

P_{1 \mid 0} = \begin{bmatrix} 1 & \Delta t \\\ 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} 0 & 0 \\\ 0 & 0 \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 \\\ \Delta t & 1 \end{bmatrix} + \begin{bmatrix} \Delta t^2/2 \\\ \Delta t \end{bmatrix} \cdot \sigma_a^2 \cdot \begin{bmatrix} \Delta t^2/2 & \Delta t \end{bmatrix} = \sigma_a^2 \cdot \begin{bmatrix} \Delta t^4/4 & \Delta t^3/2 \\\ \Delta t^3/2 & {\Delta t}^2 \end{bmatrix}

注意這個時候預測的估計協(xié)方差矩陣

P_{1 \mid 0}

相比

P_{0 \mid 0}

就大一些，這主要是由于預測過程帶來的誤差,。

有了上述的兩個預測值,，假設我們得到了

0 + \Delta_t

時刻的估計值，因為假設

\mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}

,，我們在觀測值上有：

z_{1 \mid 1} = x_1

也就是說只能觀測到車的位置,，不能觀測到車的速度

\dot{x_1}

。

我們可以計算得到測量余量：

y_{1 \mid 1} = x_1 - \begin{bmatrix} 1 & 0 \end{bmatrix} \cdot \begin{bmatrix} a_0 \cdot \Delta t^2/2 \\\ \Delta t \cdot a_0 \end{bmatrix} = x_1 - a_0 \cdot \Delta t^2/2

有了測量余量,，可以得到測量余量的協(xié)方差：

S_{1 \mid 1} = \begin{bmatrix} 1 & 0 \end{bmatrix} \cdot P_{1 \mid 0} \cdot \begin{bmatrix} 1 \\\ 0 \end{bmatrix} + {\sigma_z}^2 = {\sigma_a}^2 \cdot \Delta t^4/4 + {\sigma_z}^2

接下來,，可以計算出卡爾曼增益：

K_{1 \mid 1} = P_{1 \mid 0} \cdot \begin{bmatrix} 1 \\\ 0 \end{bmatrix} / ({\sigma_a}^2 \cdot \Delta t^4/4 + {\sigma_z}^2) =\sigma_a^2 \cdot \begin{bmatrix} \Delta t^4/4 \\\ \Delta t^3/2 \end{bmatrix} / ({\sigma_a}^2 \cdot \Delta t^4/4 + {\sigma_z}^2)

有了卡爾曼增益，我們可以更新

x_{1 \mid 1}

和

P_{1 \mid 1}

：

x_{1 \mid 1} = x_{1 \mid 0} + K_{1 \mid 1} \cdot y_{1 \mid 1} = \begin{bmatrix} a_0 \cdot \Delta t^2/2 \\\ \Delta t \cdot a_0 \end{bmatrix} + \sigma_a^2 \cdot \begin{bmatrix} \Delta t^4/4 \\\ \Delta t^3/2 \end{bmatrix} / ({\sigma_a}^2 \cdot \Delta t^4/4 + {\sigma_z}^2) \cdot (x_1 - a_0 \cdot \Delta t^2/2)

上述我們就做出了一個更新階段的狀態(tài)的估計,，看到上述方程中只涉及到預測的變量和當前階段的觀測值,，接下來是更新階段的協(xié)方差矩陣估計：

5.2 算法流程抽象

公式是不是很復雜，但是只要你跟著流程來一趟,，應該能夠明白KF的整個過程,。跑完這個例子之后,，我們把整個流程抽象出來：

先決定當前系統(tǒng)的初始狀態(tài)，并根據(jù)預測方程（過程模型）得到一個下一個時刻預測的狀態(tài),；
根據(jù)預測方程中過程的誤差,，得到當前預測的協(xié)方差估計；
進入更新階段,，我們根據(jù)目前系統(tǒng)的觀測值和上一個時刻預測的狀態(tài),，從轉(zhuǎn)換方程（觀測模型）入手，得到一個測量余量,；
根據(jù)轉(zhuǎn)換方程和上個時刻預測的協(xié)方差估計,，也可以得到一個測量余量的協(xié)方差估計；
根據(jù)1)測量余量的協(xié)方差 $S_{k \mid k}$ ,、2)轉(zhuǎn)換方程 $\mathbf{H}$ 和3)上個時刻的預測協(xié)方差估計 $P_{k \mid k-1}$ ,，我們得到卡爾曼增益 $K_{k \mid k}$ ；
根據(jù)卡爾曼增益和測量余量,，我們從預測的狀態(tài)中更新優(yōu)化當前的狀態(tài)的值,，而這個值可以用來預測下一個時刻的狀態(tài)；
同樣,，我們根據(jù)1)卡爾曼增益 $K_{k \mid k}$ 和2)上個時刻的預測協(xié)方差估計 $P_{k \mid k-1}$ ,，我們把當前更新階段的協(xié)方差 $P_{k \mid k}$ 估計也得到，幫助下一時刻的卡爾曼增益計算,。

最后我們應該回過頭來看看以上講的房間溫度的例子,，來對比下這個過程。

示例代碼

給出一個matlab版本的小程序,。

N=200;w(1)=0;w=randn(1,N) % White Gaussian Noise of Prediction

x(1)=25;a=1; % prediction parameterfor k=2:N; x(k)=a*x(k-1)+w(k-1);end

V=randn(1,N); % White Gaussian Noise of Measurementq1=std(V);Rvv=q1.^2; % covariance of Measurementq2=std(x);Rxx=q2.^2;q3=std(w);Rww=q3.^2; % covariance of predictionc=0.2; % measurement parameterY=c*x+V;

p(1)=10; % initial prediction results(1)=26; % initial optimal resultfor t=2:N; p1(t)=a.^2*p(t-1)+Rww; % covariance of t-1 b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); % kalman gain s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); % optimal result p(t)=p1(t)-c*b(t)*p1(t); % covariance of tend

t=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');