卡爾曼濾波演算法的核心是動態調整權值。用過互補濾波的應該知道它的權值是靜態的，而卡爾曼是動態的。剛剛接觸卡爾曼也不要緊張，我來一步步剖析這個東西。
下面附圖的五條公式是卡爾曼的核心。它的本質就是透過預測結合測量來估計當前系統的狀態。舉個例子，假如我們要估計一架飛行器的姿態，可以透過IMU來實時測量，但是測量值有一定的風險是不準確的，所以並不能完全依賴感測器。任何一個滿足物理規律的系統應當是連續的，所以我們還可以透過上一狀態來預測當前狀態。Kalman Filter正是結合這兩條進行狀態估計，到底是相信哪一個多一點，還要根據Kt來決定，我們定義Kt為卡爾曼增益，它是根據 測量和預測的協方差來計算的。
先解釋下每個公式所要表達的含義以及變數的含義：
line 2： 首先透過上一狀態最優值和將要施加的控制量來預測當前狀態，由假設一可以得到：
因為我們只是求均值，而高斯噪聲均值為0，所以可省去最後一項。
At指當前時刻的狀態轉移矩陣（就是指從上一狀態轉變為下一狀態的關係矩陣）；
Bt指當前時刻的控制矩陣（就是指影響控制量的控制矩陣）；
ut指當前時刻的控制量；
ut-1指上一時刻最優估計值；
指當前時刻估計值；
line 3： 除了預測均值之外，我們還需要預測值的協方差來計算Kalman增益。
指上一時刻的預測值協方差矩陣；
Rt指當前時刻測量值噪聲矩陣；
line 4：根據預測值的協方差，測量值和狀態的比例係數，測量值的協方差來計算Kalman增益。
Kt指卡爾曼增益（指的就是權值）；
Ct指的是當前時刻的測量方程；
Qt表示觀察量的協方差矩陣；
line 5：這一行可以說是Kalman Filter 的精華了，現在我們有了對狀態的預測值和協方差，同時也收集到了對狀態的測量值。這時就可以透過kalman增益來計算狀態估計值了。
增益越大，表明我們越相信測量值。
line 6： 根據 line3 ，預測當前狀態需要用到上一狀態的協方差，所以我們還需要計算當前狀態的協方差用於下一次迭代。它同樣要根據Kalman增益來計算：
相信到這裡，大家應該對kalman Filter的原理有了一個大致的瞭解，演算法中，從初始狀態開始，不斷計算當前狀態的均值和方差來迭代，直至系統結束。
上面解釋了各個公式以及各個變數的含義。其實擴充套件卡爾曼的主要作用還是在不斷的迭代中求出最接近真實值的那個值。卡爾曼濾波的作用有以下兩種。第一，如果卡爾曼用作單種資料濾波（或者多種資料分開），那麼將資料作為測量量傳入模型中，卡爾曼模型會透過上一次的值估計出下一次的值，然後將此次的估計值和測量值分別取一定的權值（模型自己所計算的權值（卡爾曼增益）），求出這次的最優值。第二，是多資料的融合。可將一種資料作為測量量，另一種資料作為估計值進行融合。
卡爾曼濾波演算法的核心是動態調整權值。用過互補濾波的應該知道它的權值是靜態的，而卡爾曼是動態的。剛剛接觸卡爾曼也不要緊張，我來一步步剖析這個東西。
下面附圖的五條公式是卡爾曼的核心。它的本質就是透過預測結合測量來估計當前系統的狀態。舉個例子，假如我們要估計一架飛行器的姿態，可以透過IMU來實時測量，但是測量值有一定的風險是不準確的，所以並不能完全依賴感測器。任何一個滿足物理規律的系統應當是連續的，所以我們還可以透過上一狀態來預測當前狀態。Kalman Filter正是結合這兩條進行狀態估計，到底是相信哪一個多一點，還要根據Kt來決定，我們定義Kt為卡爾曼增益，它是根據 測量和預測的協方差來計算的。
先解釋下每個公式所要表達的含義以及變數的含義：
line 2： 首先透過上一狀態最優值和將要施加的控制量來預測當前狀態，由假設一可以得到：
因為我們只是求均值，而高斯噪聲均值為0，所以可省去最後一項。
At指當前時刻的狀態轉移矩陣（就是指從上一狀態轉變為下一狀態的關係矩陣）；
Bt指當前時刻的控制矩陣（就是指影響控制量的控制矩陣）；
ut指當前時刻的控制量；
ut-1指上一時刻最優估計值；
指當前時刻估計值；
line 3： 除了預測均值之外，我們還需要預測值的協方差來計算Kalman增益。
指上一時刻的預測值協方差矩陣；
Rt指當前時刻測量值噪聲矩陣；
line 4：根據預測值的協方差，測量值和狀態的比例係數，測量值的協方差來計算Kalman增益。
Kt指卡爾曼增益（指的就是權值）；
Ct指的是當前時刻的測量方程；
Qt表示觀察量的協方差矩陣；
line 5：這一行可以說是Kalman Filter 的精華了，現在我們有了對狀態的預測值和協方差，同時也收集到了對狀態的測量值。這時就可以透過kalman增益來計算狀態估計值了。
增益越大，表明我們越相信測量值。
line 6： 根據 line3 ，預測當前狀態需要用到上一狀態的協方差，所以我們還需要計算當前狀態的協方差用於下一次迭代。它同樣要根據Kalman增益來計算：
相信到這裡，大家應該對kalman Filter的原理有了一個大致的瞭解，演算法中，從初始狀態開始，不斷計算當前狀態的均值和方差來迭代，直至系統結束。
上面解釋了各個公式以及各個變數的含義。其實擴充套件卡爾曼的主要作用還是在不斷的迭代中求出最接近真實值的那個值。卡爾曼濾波的作用有以下兩種。第一，如果卡爾曼用作單種資料濾波（或者多種資料分開），那麼將資料作為測量量傳入模型中，卡爾曼模型會透過上一次的值估計出下一次的值，然後將此次的估計值和測量值分別取一定的權值（模型自己所計算的權值（卡爾曼增益）），求出這次的最優值。第二，是多資料的融合。可將一種資料作為測量量，另一種資料作為估計值進行融合。