最近簡單看了一下kalman濾波，這裏先寫些筆記
卡爾曼濾波說的是“兩套系統“得到的計算量或者測量量，一個是根據系統狀態預測出來的下一個狀態的各種參量，另一個是實際測量值，那麼我們怎麼在這兩套系統中找到更合適的值。
說白了，它所謂的這兩套系統都是服從高斯分佈的，他們的乘也符合高斯分佈，根據這兩個高斯分佈的方差的大小，調整到合適的方差選擇
F有的地方有人喜歡寫成A。B是控制矩陣, x應該加的是均值爲零的噪聲，所以就省略掉沒寫，B有的文獻喜歡寫成C（control），P，協方差矩陣，有的地方也喜歡寫成V的
H是觀測矩陣，是你最後的觀測值和待分析狀態之間的矩陣轉移關係,有地地方喜歡寫成C，比如 我只能觀測到所有狀態的和，那麼，測量值（z） = [1 1 1 1 1...] * [ x1 x2 x2....]T ，R 就是1*1維度的，另外 y 就是 測量餘量（誤差），注意，y 和 x 沒什麼自變量應變量之間的關係，別看到x，y就想到 y =f(x)
x 是某些狀態值（不一定能觀察到，或者是某些變量的中間傳遞函數），有點隱含狀態的意思
kalman描述的是一些原始軌跡或者規律不方便描述，但有狀態之間的轉移是瞭解的（A矩陣），另外 H 是一個轉換矩陣
它能 H * x（隱含狀態） = 觀測值
p0 和Q 都可以先設爲對角矩陣
Q 和 R的設定，記住，Q/(Q+R) 就是卡爾曼增益的收斂值
Q越大，說明測量值可靠，R越大，說明預測值越可靠
X的初始值和方差可以簡單設置，後面會收斂，難的是Q和R的值
擴展Extended kalman filter
g函數是狀態轉移函數了，不需要A矩陣了，參與運算的Gt在計算方差的時候出現
h函數是觀測函數了，Ht是H函數的求偏導
記憶公式我也不擅長，我找了兩個工具箱，分別是kalman 和擴展kalman 的代碼
做了一些改動，上傳至：
https://download.csdn.net/download/book_bbyuan/11729042
我reference的是一個很好的 KF和 EKF的例子
http://www.pyrunner.com/weblog/2018/04/12/kalman-example/
我按它給出的python的函數，自己適當改動了一下，使用matlab 改寫，下圖是我做的實驗
原問題描述的是做8字運行的小車的軌跡，下三圖分別是 x y位置，角速度，速度的真實狀態和含噪數據
首先使用kalman濾波器，我用的是
https://www.cs.ubc.ca/~murphyk/Software/Kalman/kalman.html
中剝離出來，它實際就三個函數是核心的，kalman_filter.m kalman_update.m kalman_filter.m, 我直接雜糅到一個函數裏面了
狀態：四個: x dx/dt dx2/dt2 y dy/dt dy2/dt2 ,
，x y是位置
觀測值：兩個， x_gps y_gps
得到結論如下: 黑色是實際運行軌跡，藍色點是觀測值，橙紅色是KF濾波之後結果
下面嘗試使用擴展卡爾曼濾波器
這時候狀態值還是 上面說的四元組
但觀測值，這次是四個：
使用的擴展卡爾曼濾波器，reference這裏：
https://www.cbcity.de/das-extended-kalman-filter-einfach-erklaert
https://www.mathworks.com/matlabcentral/fileexchange/18189-learning-the-extended-kalman-filter
原公式需要求各個狀態值的偏導數
jacobi行列式的原來定義
https://www.codetd.com/article/2131570 有一個例子
他的特點是不用記公式，只要描述x中的不同狀態變量之間的關係就可以
求偏導的事情交給工具就行了
黑色是實際運行軌跡，藍色點是觀測值，橙紅色是KF濾波之後結果
擴展的效果要優於一般的kalman濾波器
代碼上傳到：