參考鏈接：
這篇復制粘貼了網(wǎng)上很多講卡爾曼的文章：
https://blog.csdn.net/App_12062011/article/details/51758989
這篇是卡爾曼濾波的opencv實例：
https://www.cnblogs.com/21207-iHome/p/5238242.html
tracking的傳統(tǒng)方法的綜述：幾年前的：
?https://zhuanlan.zhihu.com/p/32453407
OpenCV寫代碼：
https://blog.csdn.net/gdfsg/article/details/50904811
MATLAB相關(guān)的卡爾曼濾波：
https://blog.csdn.net/Kerrwy/article/details/81120259
零吠谢、引入
參考網(wǎng)站：http://www.reibang.com/p/d51a3a7736ca
假設(shè)我們要研究的對象是一個房間的溫度。根據(jù)你的經(jīng)驗判斷诗茎，這個房間的溫度是恒定的囊卜，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來做時間單位）。假設(shè)你對你的經(jīng)驗不是100%的相信错沃，可能會有上下偏差幾度栅组。我們把這些偏差看成是高斯白噪聲（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時刻一樣的割去，假設(shè)是23度，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優(yōu)溫度值的偏差是3昼丑，你對自己預測的不確定度是4度呻逆，他們平方相加再開方，就是5）菩帝。然后咖城，你從溫度計那里得到了k時刻的溫度值，假設(shè)是25度呼奢，同時該值的偏差是4度宜雀。
由于我們用于估算k時刻的實際溫度有兩個溫度值，分別是23度和25度控妻。究竟實際溫度是多少呢州袒？相信自己還是相信溫度計呢揭绑？究竟相信誰多一點弓候，我們可以用他們的covariance來判斷郎哭。因為Kg=52/(52+4^2)，所以Kg=0.61菇存，我們可以估算出k時刻的實際溫度值是：23+0.61(25-23)=24.22度夸研。可以看出依鸥，因為溫度計的covariance比較泻ブ痢（比較相信溫度計），所以估算出的最優(yōu)溫度值偏向溫度計的值贱迟。
現(xiàn)在我們已經(jīng)得到k時刻的最優(yōu)溫度值了姐扮，下一步就是要進入k+1時刻，進行新的最優(yōu)估算衣吠。到現(xiàn)在為止茶敏，好像還沒看到什么自回歸的東西出現(xiàn)。對了缚俏，在進入k+1時刻之前惊搏，我們還要算出k時刻那個最優(yōu)值（24.22度）的偏差。算法如下：((1-Kg)
52)0.5=3.12忧换。這里的5就是上面的k時刻你預測的那個23度溫度值的偏差恬惯，得出的3.12就是進入k+1時刻以后k時刻估算出的最優(yōu)溫度值的偏差（對應(yīng)于上面的3）。
就是這樣亚茬，卡爾曼濾波器就不斷的把covariance遞歸酪耳，從而估算出最優(yōu)的溫度值。他運行的很快刹缝，而且它只保留了上一時刻的covariance葡兑。上面的Kg，就是卡爾曼增益（Kalman Gain）赞草。他可以隨不同的時刻而改變他自己的值讹堤，是不是很神奇！
一厨疙、公式推導和原理理解
第一個公式：
假如現(xiàn)在有一輛在路上做直線運動的小車（如下所示）洲守，該小車在 t 時刻的狀態(tài)可以用一個向量來表示，其中pt表示他當前的位置沾凄，vt表示該車當前的速度梗醇。當然，司機還可以踩油門或者剎車來給車一個加速度ut撒蟀，ut相當于是一個對車的控制量叙谨。顯然，如果司機既沒有踩油門也沒有踩剎車保屯，那么ut就等于0手负。此時車就會做勻速直線運動涤垫。
如果我們已知上一時刻 t-1時小車的狀態(tài)，現(xiàn)在來考慮當前時刻t小車的狀態(tài)竟终。顯然有
易知蝠猬，上述兩個公式中，輸出變量都是輸入變量的線性組合统捶，這也就是稱卡爾曼濾波器為線性濾波器的原因所在榆芦。既然上述公式表征了一種線性關(guān)系，那么我們就可以用一個矩陣來表示它喘鸟，則有
如果另其中的
則得到卡爾曼濾波方程組中的第一條公式——狀態(tài)預測公式匆绣，而Ft就是狀態(tài)轉(zhuǎn)移矩陣，它表示我們?nèi)绾螐纳弦粻顟B(tài)來推測當前狀態(tài)什黑。而B則是控制矩陣犬绒，它表示控制量u如何作用于當前狀態(tài)。
上式中x頂上的hat表示為估計值（而非真實值）兑凿。等式左端部分的右上標“-”表示該狀態(tài)是根據(jù)上一狀態(tài)推測而來的凯力，稍后我們還將對其進行修正以得到最優(yōu)估計，彼時才可以將“-”去掉礼华。
第二個公式：
既然我們是在對真實值進行估計咐鹤，那么就理應(yīng)考慮到噪聲的影響。實踐中圣絮，我們通常都是假設(shè)噪聲服從一個0均值的高斯分布祈惶，即Noise~Guassian(0,σ)。例如對于一個一維的數(shù)據(jù)進行估計時扮匠，若要引入噪聲的影響捧请，其實只要考慮其中的方差σ即可。當我們將維度提高之后棒搜，為了綜合考慮各個維度偏離其均值的程度疹蛉，就需要引入?yún)f(xié)方差矩陣。
回到我們的例子力麸，系統(tǒng)中每一個時刻的不確定性都是通過協(xié)方差矩陣 Σ 來給出的可款。而且這種不確定性在每個時刻間還會進行傳遞暖侨。也就是說不僅當前物體的狀態(tài)（例如位置或者速度）是會（在每個時刻間）進行傳遞的梦鉴，而且物體狀態(tài)的不確定性也是會（在每個時刻間）進行傳遞的。這種不確定性的傳遞就可以用狀態(tài)轉(zhuǎn)移矩陣來表示赛糟，即（注意埃叭，這里用到了前面給出的關(guān)于協(xié)方差矩陣的性質(zhì)）
但是我們還應(yīng)該考慮到摸恍，預測模型本身也并不絕對準確的，所以我們要引入一個協(xié)方差矩陣 Q 來表示預測模型本身的噪聲（也即是噪聲在傳遞過程中的不確定性）赤屋，則有
這就是卡爾曼濾波方程組中的第二條公式立镶，它表示不確定性在各個時刻間的傳遞關(guān)系壁袄。
第三個公式：
繼續(xù)我們的小汽車例子。你應(yīng)該注意到谜慌，前面我們所討論的內(nèi)容都是圍繞小汽車的真實狀態(tài)展開的。而真實狀態(tài)我們其實是無法得知的莺奔，我們只能通過觀測值來對真實值進行估計欣范。所以現(xiàn)在我們在路上布設(shè)了一個裝置來測定小汽車的位置，觀測到的值記為Y(t)令哟。而且從小汽車的真實狀態(tài)X(t)到其觀測狀態(tài)還有一個變換關(guān)系恼琼，這個變換關(guān)系我們記為h(?)，而且這個h(?)還是一個線性函數(shù)屏富。此時便有：
Y(t) = h[X(t)] + V(t)
其中V(t)表示觀測的誤差晴竞。既然h(?)還是一個線性函數(shù)，所以我們同樣可以把上式改寫成矩陣的形式狠半，則有
Yt=Hxt + v
就本例而言噩死，觀測矩陣 H = [1 0]，這其實告訴我們x和Z（就是h[X(t)] + V(t)）的維度不一定非得相同神年。在我們的例子中已维，x是一個二維的列向量，而Z（本例中即Yt）只是一個標量已日。此時當把x與上面給出的H相乘就會得出一個標量垛耳，此時得到的Y就是x中的首個元素，也就是小車的位置飘千。同樣堂鲜，我們還需要用一個協(xié)方差矩陣R來取代上述式子中的v來表示觀測中的不確定性。當然护奈，由于Z是一個一維的值缔莲，所以此時協(xié)方差矩陣R也只有一維，也就是只有一個值霉旗，即觀測噪聲之高斯分布的參數(shù)σ酌予。如果我們有很多裝置來測量小汽車的不同狀態(tài)，那么Z（本例中即Yt）就會是一個包含所有觀測值的向量奖慌。
接下來要做的事情就是對前面得出的狀態(tài)估計進行修正抛虫，具體而言就是利用下面這個式子
yt為實際觀察值。
第四個公式：
前面我們提到简僧，
是根據(jù)上一狀態(tài)推測而來的建椰，那么它與“最優(yōu)”估計值之間的差距現(xiàn)在就是等式右端加號右側(cè)的部分。
表示實際觀察值與預估的觀測值之間的殘差岛马。這個殘差再乘以一個系數(shù)K就可以用來對估計值進行修正棉姐。K稱為卡爾曼系數(shù)屠列，它也是一個矩陣，它是對殘差的加權(quán)矩陣伞矩，有的資料上稱其為濾波增益陣笛洛。
定性解讀如下：
濾波增益陣首先權(quán)衡預測狀態(tài)協(xié)方差矩陣Σ 和觀測值矩陣R的大小，并以此來覺得我們是更傾向于相信預測模型還是詳細觀測模型乃坤。如果相信預測模型多一點苛让，那么這個殘差的權(quán)重就會小一點。反之亦然湿诊，如果相信觀察模型多一點狱杰，這個殘差的權(quán)重就會大一點。
不僅如此厅须，濾波增益陣還負責把殘差的表現(xiàn)形式從觀測域轉(zhuǎn)換到了狀態(tài)域仿畸。例如本題中觀測值Z（本例中即Yt）僅僅是一個一維的向量，狀態(tài) x （包含小車的位置和速度）是一個二維的向量朗和。所以在實際應(yīng)用中错沽，觀測值與狀態(tài)值所采用的描述特征或者單位都有可能不同，顯然直接用觀測值的殘差去更新狀態(tài)值是不合理的眶拉。而利用卡爾曼系數(shù)甥捺，我們就可以完成這種轉(zhuǎn)換。例如镀层，在小車運動這個例子中镰禾，我們只觀察到了汽車的位置，但K里面已經(jīng)包含了協(xié)方差矩陣sigma的信息（sigma里面就給出了速度和位置的相關(guān)性）唱逢，所以它利用速度和位置這兩個維度的相關(guān)性吴侦，從位置的殘差中推算出了速度的殘差。從而讓我們可以對狀態(tài)值 x 的兩個維度同時進行修正坞古。
詳細推導參考文獻：【1】秦永元备韧，張洪鉞，汪叔華痪枫，卡爾曼濾波與組合導航原理织堂，西北工業(yè)大學出版社? 的第35-37頁
第五個公式：
最后，還需對最優(yōu)估計值的噪聲分布進行更新奶陈。所使用的公式為
總結(jié):
至此易阳，我們便獲得了實現(xiàn)卡爾曼濾波所需的全部五個公式：
我們將這五個公式分成預測組和更新組。預測組總是根據(jù)前一個狀態(tài)來估計當前狀態(tài)吃粒。更新組則根據(jù)觀測信息來對預測信息進行修正潦俺，以期達到最優(yōu)估計之目的。
整個卡爾曼濾波的算法基本如下：
參考網(wǎng)頁：https://zhuanlan.zhihu.com/p/21692854
二、MATLAB簡單實例
當然事示，你可能困惑于卡爾曼濾波是否真的有效早像。下面利用參考文件【2】卡爾曼濾波的原理以及在MATLAB中的實現(xiàn)，http://blog.csdn.net/revolver/article/details/37830675 中給出的例子來演示卡爾曼濾波的威力肖爵。這個例子模擬質(zhì)點進行勻速直線運動（速度為1）卢鹦，然后引入一個非常大的噪聲，再用卡爾曼濾波來對質(zhì)點的運動狀態(tài)進行軌跡劝堪。注意是勻速直線運動冀自，所以其中不含有控制變量（即加速度）。
%觀測值(含噪聲)
Z = (1:100);%真實值
noise = randn(1,100);
Z = Z +noise;
%狀態(tài)值的初始化
X = [0;0];%二維幅聘，代表起始時凡纳，該點的位置和速度
Sigma = [1 0 ; 0 1];%狀態(tài)轉(zhuǎn)移協(xié)方差矩陣
F = [1 1;0 1]; %delta t = 1
Q = [0.0001 0;0 0.0001];%狀態(tài)轉(zhuǎn)移協(xié)方差矩陣的噪聲矩陣
H = [1 0];
R = 1;
figure;
hold on;
for i = 1:100
? X_ = F*X;
? Sigma_ = F*Sigma*F'+Q;
? K = Sigma_*H'/(H*Sigma_*H'+R);
? %/的速度比inv快
? X = X_ +K*(Z(i) - H* X_);
? Sigma = (eye(2) - K*H)*Sigma_;
? %eye是對角線元素為1的矩陣
? plot(X(1),X(2),'.','MarkerSize',10)
? %畫點窃植，橫軸表示位置帝蒿，縱軸表示速度。
end
plot([0,100],[1,1],'r-')
結(jié)果如下：
下圖給出了上述代碼的執(zhí)行結(jié)果巷怜「鸪可見經(jīng)過最開始的幾次迭代后，質(zhì)點運動的狀態(tài)估計就回到了正確軌跡上延塑，而且估計的結(jié)果基本圍繞在真實值附近绣张，效果還是很理想的。
三关带、用MATLAB進行小球位置預測
參考網(wǎng)站：https://www.mathworks.com/help/vision/examples/using-kalman-filter-for-object-tracking.html
中文翻譯版本：https://blog.csdn.net/Kerrwy/article/details/81120259
四侥涵、用MATLAB進行多目標位置跟蹤
https://www.mathworks.com/help/vision/examples/motion-based-multiple-object-tracking.html
五、用卡爾曼濾波進行自己的數(shù)據(jù)的跟蹤
1.把待測數(shù)據(jù)通過Unet test宋雏，得到經(jīng)過Unet segment之后的分割圖
（1）用label的圖片或者segmented的圖片作為input芜飘，我暫時用tif格式的。
（2）直接下載singleball.mp4跑一下源代碼
遇到問題1：無法調(diào)用matlab自定義函數(shù)
參考網(wǎng)站：https://www.ilovematlab.cn/thread-134014-1-1.html
解決辦法：新建的時候新建錯了吧 不要新建 script文件 可以選擇新建function的 就能使用定義function了磨总。成功解決嗦明。
遇到問題2：報錯未定義變量 "utilities" 或類"utilities.videoReader"。
解決辦法：在對應(yīng)子函數(shù)的開頭加上：param = getDefaultParameters(); utilities = createUtilities(param);
遇到問題3：未定義函數(shù)或變量 'frame'蚪燕，?'detectedLocation'娶牌，。
解決辦法：global一下frame變量
遇到問題4：tracking部分第12行報錯：matlab運算符異常馆纳。
解決辦法：global utilities.foregroundMask 這樣不行
各種bug诗良。。鲁驶。累榜。。。
六壹罚、卡爾曼濾波器調(diào)參需要注意的事項
應(yīng)根據(jù)探測器的精度選擇MeasurementNoise的值葛作。 測量噪聲設(shè)置為較大的值會獲得不太精確的檢測器。 以下示例說明了錯誤配置的分段閾值的噪聲檢測猖凛。 增加測量噪聲會導致卡爾曼濾波器更多地依賴于其內(nèi)部狀態(tài)而不是輸入的測量值赂蠢，從而補償檢測噪聲。
通常辨泳，物體不會以恒定的加速度或恒定的速度移動虱岂。 您可以使用MotionNoise指定與理想運動模型的偏差量。 當您增加運動噪聲時菠红，卡爾曼濾波器更多地依賴于輸入測量而不是內(nèi)部狀態(tài)第岖。 嘗試使用MotionNoise參數(shù)進行試驗，以了解有關(guān)其效果的更多信息试溯。
七蔑滓、試試這個toolbox
https://github.com/rlabbe/filterpy
安裝：pip install filterpy
介紹：Python Kalman filtering and optimal estimation library. Implements Kalman filter, particle filter, Extended Kalman filter, Unscented Kalman filter, g-h (alpha-beta), least squares, H Infinity, smoothers, and more. Has companion book 'Kalman and Bayesian Filters in Python'.
中文翻譯在這里：https://github.com/loveuav/Kalman-and-Bayesian-Filters-in-Python
先讀設(shè)計這一章：
