https://mp.weixin.qq.com/s/cr1u840FeBLalrmtJQs8TQ
本篇文章分上最铁、中、下三篇开缎，上篇從標(biāo)準(zhǔn)卡爾曼濾波開始斤寂，中篇加入更真實的系統(tǒng)模型，下篇從傳感器的數(shù)據(jù)融合中實現(xiàn)擴展卡爾曼濾波诚镰。
8. 一個更真實的模型
現(xiàn)在重新回顧一下描述系統(tǒng)的狀態(tài)方程和觀測方程：
表示系統(tǒng)的當(dāng)前狀態(tài)奕坟，
表示系統(tǒng)的前一個狀態(tài)；
是一個常量清笨，
是系統(tǒng)當(dāng)前狀態(tài)的觀測值月杉；
是系統(tǒng)觀測噪聲；
這兩個方程已經(jīng)適用于大多數(shù)系統(tǒng)函筋，但仍然不夠普適性沙合；現(xiàn)在依然以飛機的飛行為例奠伪，我們并沒有考慮到飛行員對飛機的操作和控制跌帐，飛行員操作控制桿向前或向后移動，對飛機輸入控制量绊率，最終對飛機產(chǎn)生控制谨敛。我們將這個控制量稱為，表示飛行員發(fā)送到飛機的控制信號的當(dāng)前值滤否，并且我們對這個控制量加上一個縮放因子
脸狸，因此整個狀態(tài)方程變?yōu)椋?/p>
一般而言，除了噪聲以外的任何信號都可以通過常量進行一定比例的縮放，因此觀測方程也可以這樣表示：
9. 調(diào)整估計值
通過以上的方程我們更真實的描述一個系統(tǒng)的狀態(tài)和觀測炊甲，由此預(yù)測和更新方程也需要做相應(yīng)的調(diào)整：
預(yù)測：
更新：
仍然以飛機飛行為例泥彤，我們增加一個控制信號，表示飛行員穩(wěn)步拉回控制桿卿啡，并提高飛機飛行高度吟吝，原始信號為藍色表示，觀測值以紅色表示颈娜，綠色信號為卡爾曼濾波后的值剑逃。
10. 向系統(tǒng)中添加速度
現(xiàn)在回想一下飛機下降時飛行高度的原始方程：
以更通用的方程表示為：
我們都知道飛機飛行的高度即海拔，可以看做是一種距離官辽，距離總是和速度與時間相關(guān)的：
再進一步思考蛹磺，當(dāng)前距離和前一時刻的距離的關(guān)系：
下標(biāo)代表當(dāng)前，
代表前一個同仆。也就是說萤捆，飛機現(xiàn)在的位置等于前一時刻的位置加上剛剛飛行過的垂直距離。如果按照固定的時間間隔進行計算俗批，比如說1秒鳖轰，或者1分鐘等，那么可以將上式簡化為：
貌似與我們的狀態(tài)方程更接近了一步扶镀，但是還差一些蕴侣，接下來我們引入向量和矩陣。
11. 向量和矩陣
通常情況下系統(tǒng)的狀態(tài)并不是一個變量臭觉，比如飛機的高度和速度昆雀，那么我們就可以使用向量表示系統(tǒng)的狀態(tài)：
符號表示一個定義，系統(tǒng)狀態(tài)定義為一個距離和速度的矢量蝠筑。 當(dāng)我們用一個矩陣去乘以一個向量時得到的結(jié)果依然是一個向量狞膘，比如：
因此，我們將系統(tǒng)狀態(tài)方程中的常量定義為一個矩陣什乙，因為系統(tǒng)的狀態(tài)并非是一種：
再次回顧一下系統(tǒng)狀態(tài)方程：
將系統(tǒng)狀態(tài)矢量代入：
從公式中可以清晰的看出距離等于前一刻的距離與速度和觀測時間間隔的乘積之和挽封，而速度和上一時刻的速度相等；這表明飛機是勻速上升或下降的臣镣，但現(xiàn)實中速度不可能做到勻速辅愿。當(dāng)速度是變化的，即系統(tǒng)中還存在一個加速度變量忆某，那么我們的系統(tǒng)狀態(tài)方程可以表示為：
12. 再看預(yù)測和更新
系統(tǒng)狀態(tài)的修正公式：
為一個向量点待，
是一個矩陣；大家可能還記得弃舒，這個方程的原始形式是這樣的：
其中是一個控制信號癞埠，
是一個比例因子状原，系統(tǒng)的觀測方程為：
其中是觀測量，
是由于傳感器的不準(zhǔn)確所帶來的噪聲苗踪。那么我們?nèi)绾涡拚@些方程以適應(yīng)新的向量矩陣形式呢颠区？是的，線性代數(shù)讓這些變得尤其簡單通铲。我們使用大寫的字母寫出縮放因子
和
瓦呼，使他們成為矩陣，而不是單一的標(biāo)量值：
然后所有的變量包括狀態(tài)测暗、觀測央串、噪聲、控制等都被認為是向量碗啄，回顧一下我們前面的系統(tǒng)預(yù)測和更新方程：
預(yù)測：
更新：
大家可能會想质和，將這里所有的縮放因子都用大寫表示，然后就可以了稚字。然而事實并非如此饲宿，矩陣的乘法沒有那么簡單，這就是前面提到的為什么是而不是
胆描。
有時候不等于
瘫想，有時候需要轉(zhuǎn)置矩陣，通過在矩陣的旁邊加上一個上標(biāo)
來表示轉(zhuǎn)置矩陣昌讲。轉(zhuǎn)置矩陣是通過把每一行變成一列国夜，每一列變成一行來實現(xiàn)的《坛瘢看個實際例子：
由此车吹，我們可以重寫我們的預(yù)測方程: (關(guān)于卡爾曼濾波五個公式的由來，我會專門寫一篇文章進行推導(dǎo)與分析)
我們已經(jīng)知道為何是一個矩陣醋闭，但是
為何也是一個矩陣呢窄驹？
我們的第二個更新方程可以表示為：
再看增益方程的原始形式：
它包含一個除法，我們瞬間想到了除法可以用乘法代替：
那么上面的方程可以寫為：
我們現(xiàn)在把這些常量替換成矩陣：
如何計算呢证逻，求逆矩陣嗎乐埠？是的，一個矩陣乘以它的逆矩陣后結(jié)果為一個單位矩陣
單位矩陣的定義如下囚企，我們以一個 3x3 的矩陣舉例：
13. 傳感器融合簡單介紹
現(xiàn)在我們的卡爾曼濾波方程完全以矩陣形式表示丈咐，
模型：
預(yù)測：
更新：
如果想在狀態(tài)變量中添加以下額外的項似乎是一項很艱巨的任務(wù)《床Γ卡爾曼濾波最有價值的地方就是用在傳感器的融合中扯罐。
回到飛機飛行的例子中负拟，飛行員可以訪問更多的信息烦衣，不僅僅是飛機的高度，儀表上可以顯示飛機的水平速度、航向花吟、經(jīng)緯度秸歧、室外溫濕度等信息。想象一下衅澈，假如一架飛機只有三個傳感器键菱，每一個傳感器都有給定的狀態(tài)，一個用于測量高度的氣壓計今布，一個用于航向的電子羅盤经备，一個測量空速的空速指示器等。
假設(shè)這些傳感器是完全準(zhǔn)確的部默，即沒有任何噪聲侵蒙。那么我們的觀測方程為：
將會變?yōu)椋?/p>
假如我們向系統(tǒng)中加入了一個GPS，同樣用于測量高度傅蹂，那么我們的觀測方程將會變?yōu)椋?/p>
因此可以將多個傳感器的讀數(shù)結(jié)合起來纷闺，從而推斷出某個狀態(tài)的信息。就比如我們看病一樣份蝴，對于重要的疾病總是尋求多個醫(yī)生的診斷犁功，對于一些重要的事情，最好有多個信息來源婚夫。
本篇先介紹這些浸卦，下一篇會以一個真實的傳感器融合實例開始，并最終實現(xiàn)擴展卡爾曼濾波案糙。
