簡單的insignt

狀態(tài)估計方程

方程中干厚，下標(biāo)上的k是狀態(tài)礁蔗。在這里我們可以將其視為離散的時間間隔壤短，例如k=1表示1ms设拟，k=2表示2ms。

我們的目標(biāo)是找到狀態(tài)k處的X的估計值久脯，即信號X的估計值纳胧。其中，Z_k是測量值帘撰，K_k稱為卡爾曼增益跑慕，x_k-1是對先前狀態(tài)的信號的估計。因為我們有了測量值，并且已經(jīng)有了先前的估計信號核行。唯一不確定的量是K_k牢硅，即卡爾曼增益。我們應(yīng)該為每個后續(xù)狀態(tài)計算卡爾曼增益芝雪。

另一方面减余，假設(shè)K_k是0.5， 那么我們得到了什么呢绵脯？這是一個簡單的平均數(shù)佳励，換句話說，我們應(yīng)該在每個狀態(tài)下找到更智能的卡爾曼增益系數(shù)蛆挫。最關(guān)鍵的是：卡爾曼濾波器為每個隨后的狀態(tài)找到最優(yōu)的averaging factor赃承。同樣以某種方式對過去的狀態(tài)保留一些記憶。

step-by-step 指南

step1-構(gòu)造一個模型

卡爾曼濾波器有如下方程：

方程

這意味著我們可以使用線性隨機方程(第一個) 來評估每個x_k悴侵。任何x_k都是其先前狀態(tài)x_k-1加上一個控制信號u_k加上一個過程噪聲w_k-1的線性組合瞧剖。在大多數(shù)情況下，沒有控制信號u_k可免。

第二個方程告訴我們?nèi)魏斡^測值z_k都是信號值x_k和測量噪音v_k的線性組合抓于。他們都被認為是高斯分布的觉既。過程噪聲和測量噪聲在統(tǒng)計上是獨立的谣旁。A, B和H是一般形式的矩陣。在大多數(shù)信號處理問題中野舶，這些量只是數(shù)值妇垢。盡管這些值在狀態(tài)之間可能會發(fā)生變化巾遭，但是在大多數(shù)情況下，我們可以假定它們是恒定的闯估。

如果我們的系統(tǒng)適合該模型灼舍，剩下的唯一事情就是估計噪聲函數(shù)w_k-1和v_k的均值和標(biāo)準(zhǔn)差。我們知道涨薪，在現(xiàn)實生活中骑素，沒有信號是純粹的高斯信號，但是我們可以近似地假設(shè)它刚夺。這不是一個大問題献丑，因為即使高斯噪聲參數(shù)的估算不佳，我們也會看到卡爾曼濾波算法試圖收斂到正確的估算中侠姑。

step2-開始過程(start the process)

下一步是確定必要的參數(shù)和初始值阳距。在這個過程中，有兩個方程：Time Update(prediction) 方程和 Measurement Update(correction) 方程结借。這兩個方程都應(yīng)用在每個第k個狀態(tài)筐摘。

我們在step1中進行了建模，因此我們知道矩陣A，B和H的值咖熟，大多數(shù)情況下圃酵，它們是數(shù)值常數(shù)。 甚至在大多數(shù)情況下馍管，它們等于1郭赐。剩下最痛苦的事情是確定R和Q，R很容易找出确沸，因為我們對環(huán)境中的噪聲很確定捌锭。但是找出Q并沒有那么顯而易見。為了開始這個過程罗捎，我們還需要知道x_0和P_0的估計值观谦。

step3-迭代

在我們收集了所需要的所有信息并開始了這個過程之后，現(xiàn)在我們可以迭代估計桨菜。先前的估計將是當(dāng)前狀態(tài)的輸入豁状。

在這里，Time Update中第一個公式等號左邊的x_k倒得，是prior estimate泻红，表示在measurement update(correction) 之前的粗略估計，第二個公式等號左邊的P_k是prior error covariance霞掺。在Measurement Update方程中谊路，我們確實找到了在時間k處的x估計值，這是在時間k處x的估計值（我們希望找到的東西）菩彬。我們評估的卡爾曼增益（Kalman Gain）在下一個迭代步驟中是不需要的凶异，它是這組方程組中隱藏，神秘且最重要的部分挤巡。

在Measurement Update中評估的值也被稱為后驗值。

一個簡單的例子

現(xiàn)在酷麦，讓我們嘗試估計一個標(biāo)量隨機常數(shù)矿卑，例如從電源獲得的“電壓讀數(shù)”。假設(shè)它具有恒定的 a 伏特沃饶，但是在a 伏特上下浮動母廷，假設(shè)測量噪聲的標(biāo)準(zhǔn)差是0.1伏。模型構(gòu)建如下:

所以這個問題被歸為一個簡單的形式糊肤。

1. 最重要的是琴昆，我們有一個一維信號問題，因此模型中的每個實體（A馆揉，B业舍，H）都是一個數(shù)值，而不是一個矩陣。
2. 沒有控制信號u_k
3. 由于信號是一個常數(shù)舷暮，A為1态罪，因為我們已經(jīng)知道下一個值將與前一個值相同。
4. 值H = 1下面，因為我們知道測量是由狀態(tài)值和一些噪聲組成的复颈。 很少會遇到現(xiàn)實生活中H不同于1的情況。

最后沥割，假設(shè)有以下觀測值：

我們需要給出x_0和P_0耗啦，假設(shè)x_0=0并且P_0=1，為什么不選擇P_0=0机杜？因為如果選擇這種方式帜讲，則意味著環(huán)境中沒有噪音，并且該假設(shè)將導(dǎo)致狀態(tài)k的所有隨之而來的x估計都為0(保持最初的state)叉庐。Time Update和Measurement Update方程計算如下：

在這里舒帮，詳細顯示了前2個狀態(tài)迭代，其他的則遵循相同的模式陡叠。我們可以看到在經(jīng)過一系列迭代之后玩郊，算法收斂到了真實值。

為了以更少的步驟使得估計收斂枉阵，你應(yīng)該：

1. 對系統(tǒng)進行更優(yōu)雅地建模
2. 更精確地估計噪聲