卡爾曼濾波器是如何運用於多感測器融合的？

我目前認為的卡爾曼濾波器是這樣的：
1.設置狀態轉移矩陣
2.[預測] 通過 t-1 時的狀態預測 t 時的狀態
3.[更新] 用感測器的測量值與 2 中的預測值估計出最優值，並更新卡爾曼增益等常數
4.重複 2～3 步驟
（如上正是opencv庫中自帶的卡爾曼濾波器例子的步驟，也是初學者能最直觀理解到的，它只涉及到一個測量值）
那麼請問，在某些涉及多感測器融合的問題中，例如 陀螺儀（累積誤差）和電子羅盤（干擾）的數據融合，此時有多個測量值存在，卡爾曼濾波器又是怎樣工作的呢？

---

請恕我直言，現有的三個答案都沒答到題主問的點子上。題主問的是多感測器使用卡爾曼濾波進行信息融合，而不是單個感測器，也不是經典卡爾曼濾波器得原理。

多感測器指的是系統觀測方程不只一個，相當於經典方法，在算新息得時候演算法不一樣。具體內容我稍後填坑

更新。。。。。。。。。。。。。。。。。。。。。。
首先說題設，先看題設，卡爾曼濾波的多感測器信息融合指的是有以下系統：

w是建模雜訊。和傳統系統最大的區別是，它的觀測方程是有n個的，大於一個，並且每個感測器測到的量不一樣，這個感測器上的雜訊的統計特性也可以是不一樣的。

而回頭看單感測器的觀測器：

卡爾曼濾波的關鍵就是這個方程里的L（即卡爾曼增益）是怎麼選取。到了多感測器的情況下，問題在於：
(1)不能直接算了，因為維度不一樣。
(2)怎麼選取最優增益L。

和現在大家討論的不同的是，它的預測還是使用

計算出來的，而並非是用感測器A做預測，使用感測器B做新息。而是感測器A和B的觀測數值都給算到新息裡面去了

細節公式還是建議大家直接去看paper吧，還是比較長的，我就不往上敲了。
Multi-sensor optimal information fusion Kalman filter

---

大家貌似有分歧，其實沒有。

本質是一樣的。因為本質是動態反饋加權，而加法具有交換率與結合率。

可以先把兩個感測器融合，融合之後的當作新息，再與系統預測融合。如果兩個感測器都是靜態，可能不會產生混淆；如果有一個是動態的，比如需要積分，本身也可算是感測器系統預測，則兩個感測器融合的過程，本身就像卡爾曼濾波的一次迭代了。這樣可能是大家分歧的原因。

也可以系統預測先融合一個感測器信新息，再融合另一個。只是如有動態感測器，會較複雜。

也可以整體來做，三個一起融合。

本質是一樣的。但需注意加權權重，別繞進去了。

---

我在做畢設的時候有用kalman濾波得角度的演算法，濾波程序不是我自己寫的，但對kalman濾波也算是弄個半懂了。主要是沒弄太懂參數、矩陣是如何得到的。

我用的是mpu6050，是個六軸的陀螺儀（3軸加速度+3軸叫速度，也就是三個方向的角速度+加速度），由兩個軸的角加速度可以得到角度，但是抖動什麼的對它的干擾太大，角速度積分也可以得到角度，但是因為是積分，會有累積誤差。簡單來說，由角速度得到的角度和加速度得到的角度都不是真實的角度值，需要將兩者進行一定比例的融合，kalman濾波就是做這事的。kg（卡爾曼增益）決定兩種方法得出的角度哪個可信度比較高，最後按比例將兩者數據加在一起。

先簡單這麼說說，等有空了再貼具體的過程，論文還在我電腦里。
-------------------------------------------------------------------------------------------------------------------------------------------
我回來粘論文了！我看lz的提問，想必卡爾曼濾波的原理應該懂得差不多了，我就不再多贅述了。主要講講在實際中的運用了。我是學電子的，畢設做的東西有實物，不是純軟體、做些模擬的。

先介紹我做的東西的硬體基礎吧，主要是兩個晶元（或者可以說是晶元組成的外圍電路）--MPU6050和stm32。MPU6050上面介紹了，不再說了，stm32 就是一單片機，卡爾曼的濾波就在stm32里處理。簡單來說就是：
MPU6050得到的角度（兩個方向的加速度經過反三角函數得到的）和一個方向的角速度送到stm32里進行卡爾曼濾波，得到最優估計的角度。

-----------------------------------------------------------------------------------------------------------------------------------------------------
-----------------------------------------------------------------------------------------------------------------------------------------------------
在這兒，我介紹一下濾波的必要性，開頭也簡單說明了。如果不想看這點的人可以略過。而且大部分的東西都是找到的資料里介紹的。

雖然MPU6050可以讀出X軸、Y軸和Z軸的加速度，理論上是可以根據X軸和Z軸加速度由式（3-1）得出小車的傾角。

(3-1)

但在實際情況中，靜止時MPU6050輸出的角度信息雜訊非常大；並且加速度計的輸出實際包括a方向的加速度和重力加速度分量的疊加。這些干擾信號疊加在測量信號中會使輸出信號無法得出準確的角度，如圖3-1所示。

圖3-1 小車運動過程中引起的加速度信號波動

小車行駛時產生的加速度使得輸出加速度在實際傾角附近波動。這些雜訊雖然可以通過數據平滑濾波將其濾除，但這一方面會使系統的實時性變差(無法及時輸出當前角度值）；另一方面，平滑濾波也會將角速度變化信息率除掉。因此，為了得到所需要的準確的平衡角度信息，需要融合MPU6050的陀螺儀數據。

陀螺儀可以得到物體當前運動的角速度。對角速度積分就可以得到角度。角速度由於不會受到小車運動的影響，因此該信號的雜訊信號很小，得到的角度信號也就跟平穩。然而角速度經過積分運算後，角度信號中存在的微小偏差和飄逸經過積分運算後會逐漸累計。隨著時間的延長，誤差逐步增加，最終導致電路飽和，無法得到正確的信號，如圖3-2。

圖3-2 角度積分漂移現象

為了消除這個誤差，在此採用卡爾曼濾波方法。加速度計輸出和陀螺儀輸出是互相彌補的關係:加速度計科院彌補陀螺儀輸出零點受溫度影響的缺陷，使積分輸出信號的誤差大大減少；而陀螺儀則可以減少加速度計輸出雜訊，加大的降低了靜態輸出雜訊。

-----------------------------------------------------------------------------------------------------------------------------------------------------
-----------------------------------------------------------------------------------------------------------------------------------------------------
然後這兒就是卡爾曼濾波的具體運用了！！！

卡爾曼濾波主要是運用5個方程（推導看了半天…………真心看的暈了）：兩個預測方程和3個更新方程。
預測方程：（公式就直接截圖我論文里打的了，公式編輯器打的公式複製不過來）

更新方程：

-----------------------------------------------------------------------------------------------------------------------------------------------------
在這兒粘些簡單的文字性原理描述：

卡爾曼濾波器在線性系統中產生最優估計，因此，感測器或者系統必須是(接近)線性系統才能被運用卡爾曼濾波。因為僅僅依靠上一次系統狀態和定義系統狀態被修正概率的方差矩陣，所以卡爾曼濾波器不需要很長的系統狀態歷史來經行濾波。這點在可確保系統的高實時性要求。

卡爾曼濾波有兩類方程:預測方程和更新方程。預測方程根據之前的狀態和控制量預測當前狀態；更新方程表示相信感測器數據多些還是相信總體估計值多些（由卡爾曼增益Kg決定）。濾波器的大致工作原理為：根據預測方程預測當前狀態並用更新方程檢測預測結果，該過程一直在重複更新當前狀態。預測方程和更新方程的關係如圖3-3所示。

圖3-3 預測方程與更新方程關係

這5個方程的關係就簡單用一個流程圖介紹下了：

-----------------------------------------------------------------------------------------------------------------------------------------------------
lz最想看的可是這一段了！！！！具體的kalman濾波代碼：

float K1 =0.02;
float angle, angle_dot;
float Q_angle=0.001;// 過程雜訊的協方差
float Q_gyro=0.003;//0.003 過程雜訊的協方差 過程雜訊的協方差為一個一行兩列矩陣
float R_angle=0.5;// 測量雜訊的協方差 既測量偏差
float dt=0.005;//
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
void Kalman_Filter(float Accel，float Gyro)
{
angle+=(Gyro - Q_bias) * dt; //先驗估計
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先驗估計誤差協方差微分的積分
PP[0][1] += Pdot[1] * dt; // =先驗估計誤差協方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - angle; //zk-先驗估計
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1]
PP[0][0] -= K_0 * t_0; //後驗估計誤差協方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
angle += K_0 * Angle_err; //後驗估計
Q_bias += K_1 * Angle_err; //後驗估計
angle_dot = Gyro - Q_bias; //輸出值(後驗估計)的微分=角速度
}


因為每個人有不同的矩陣運算定義，這裡就將矩陣的運算用數組來進行運算處理了。

在這裡看代碼的兩個入口參數樓主就知道kalman濾波的運用了吧。不過還要在此進行解釋一下！！！！

代碼里的卡爾曼濾波有一個周期dt !!!!! 這就要設計到和硬體結合了。
因為MPU6050（陀螺儀）採集數據有個採樣頻率，採樣頻率設置為200，也就是周期0.005s。MPU6050每次採樣結束，就會產生一個中斷，這個中斷告訴stm32該進行卡爾曼濾波了。dt我的理解，不僅僅是簡單的kalman的濾波周期，同時在底下的（4-1）式中，有一個乘dt的操作，這也是得角度的一個過程（角速度乘時間為角度）。

---------------------------------------------------------------------------------------------------------------------------------------------------
接下來就是代碼的解釋了。

卡爾曼濾波代碼主要根據式（3-2）~（3-6）5個公式來寫的。

在此，以Y軸角速度在時間上的積分為預測值的控制量，即U(k)=Gyro_Y；俯仰角（pitch=arctan(ACCEL_X/ACCEL_Z)）作為觀測值，即Z(k)=pitch。

因為需要同時得到Y軸的角速度和pitch角，所以進行卡爾曼濾波時需要估計兩個值，一個是pitch角，另一個是陀螺儀漂移Q_bias。根據式（3-2）建立角度測量模型方程：

（原諒我要直接截圖了，公式編輯起來實在麻煩）

我寫的對代碼的解釋可能看起來不是那麼清晰，lz也可以參考百度文庫上的一篇文章。內容差不多，就排版方式的問題了。
卡爾曼濾波C代碼分析

--------------------------------------------------------------------------------------------------------------------------------------------
最後也再來加兩張濾波效果圖。數據是用串口傳送到上位機上，然後用matlab畫了個圖

圖 5-1 靜態時角度濾波效果

圖5-2 動態時角度濾波效果

個人感覺還是靜態時候的濾波效果比較好些，動態效果也還行，但是感覺沒那麼明顯。其實我也只是看著個圖自己理解下而已，怎麼分析還是不大懂。

然後po一張最後的實物圖

----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
完！後續有問題再補充！

---

卡爾曼濾波包括擴展卡爾曼濾波也只是想這麼做，它們基本上做不到這一點。

我估計答主是想看類似這樣的
Complementary filter design on the special orthogonal group SO (3)
以上提供了一種determinstic filter，但是很容易改造成卡爾曼濾波。

另外，千萬不要看 Probabilistic Robotics上卡爾曼濾波或者擴展卡爾曼濾波的推導，正確的思路都被淹沒在冗餘的矩陣計算中，並不是好的示範。

---

說個實例吧！
我們的無人車在行駛時需要穩定的航向角數據，有兩種方法：第一種是GPS得到的，更新率0.2s，沒有累積誤差，低速時雜訊很大，高速時比較準確，第二種是通過橫擺角速度積分得到，更新率0.02s，存在累積誤差，雜訊保持不變。GPS相當於你的電子羅盤，橫擺角速度感測器相當於你的陀螺儀。
橫擺角速度積分就是預測，GPS得到的數據就是觀測，然後kalman就能運行起來了，Q、R的選擇需要根據工況而定。
貼上Simulink模型圖

至於效果，再貼幾張圖
車速為0的時候

正常行駛的時候

你自己判斷一下哪個是觀測值，哪個是預測值，哪個是kalman濾波得到的值。

---

多感測器融合，最經典實用的方面就是組合導航。卡爾曼濾波的原理有大神們闡述，是基礎演算法。
多感測器融合的核心其實是如何建立濾波所需的狀態方程和觀測方程。在組合導航中，有直接法和間接法，其思路有所不同。
直接法是直接用信號過卡爾曼做的濾波融合，間接法是通過誤差方程來做的。

當然，這些方法也可以推廣到各種方面去，如溫度測量等等。

其實在這裡說一百句都不如直接去看書來的快，來的清楚，來的權威，來的可靠。

---

參見這一答案 如何通俗並儘可能詳細解釋卡爾曼濾波？ - 機器人

融合多個感測器測量結果關鍵在於構建巧妙的觀測矩陣及其協方差,使得KF演算法具有該能力.

---

Our framework has its own inertial state
prediction, this is to say that even though there is not any new information
come in our system, our system will keep the navigation with its own state.
This design has great advantages because the prediction stage isn』t driven by
any measurement, namely, our navigation system doesn』t depend on any sensor and
this will improve its robustness. Another advantage is that any input for this
system will be seen as the same because all of them will be considered as the
measurement input to update the state of EKF framework. As a result, there is only
tpredict (the time when we use equations of evolution) as the control input in
the part of prediction in Kalman Filter, you can see the illustration as
following:

所以看得出，那相當於就是多感測器融合了，你可以在軸上加各種感測器進去

其實和各位答主的有些共同點

我把所有感測器都拿去做觀測值了

我是把他們幾個一起融合（可以嘗試用其他答主給的paper里的協方差，我目前單純地使用那卡爾曼的協方差P矩陣的計算公式）

不同是我在融合里加了模糊規則來保證平滑效果，還加了個coherence threshold，保證觀測值和觀測值預測的相關性。

好吧，新人求輕噴

---

卡爾曼是匈牙利當代著名數學家，Kalman濾波器源自於他的博士畢業論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。

卡爾曼濾波器是一個最優化自回歸數據處理方法，它是一個時域濾波器，是通過對時域上包含雜訊的有限測量數據，計算出最接近實際值的方法。這裡說它是一種遞歸的估計，是指只要獲得上一時刻的狀態值以及當前的狀態觀測值就可以計算出當前狀態的估計值，因此不需要記錄觀測或者估計的歷史信息。既然是一個時域濾波器，也就無需像低通濾波器那樣，需要在頻域進行濾波器設計而需要再轉換到時域來實現。卡爾曼濾波器的典型應用是從一組包含雜訊的對無物體位置的觀察序列中預測出物體的位置坐標和速度，它的核心內容是5個計算公式，然而，僅僅給出5個計算公式顯得過於抽象，不便於理解。這裡，為了更加形象、更容易地理解卡爾曼濾波器，我給出網上摘取的一個例子來一步一步探索卡爾曼濾波器的原理。舉例說明卡爾曼濾波器的原理

假設被研究對象是某個房間內的溫度（當然，這裡假設該房間內各處的溫度是相等的，房間內不存在空調、風扇、暖氣等局部發熱或散熱的設備）。根據生活經驗，你認為這個房間內的溫度是恆定的。即，下一時刻的溫度等於當前時刻的溫度。但是，你的生活經驗並不是絕對的準確，所以，你確信自己對房間內溫度的估計存在上下幾度的偏差。這個偏差在這裡假設是高斯白雜訊的，即：偏差跟時間沒有關係而且是服從高斯分布的。另外，在房間內放置一個溫度計，用於測量房間內的溫度。但是，由於製造工藝等因素，溫度計的測量值也不是絕對準確的，即：溫度計的測量值與真實值之間也存在一定的偏差，這個偏差也假設是高斯白雜訊的。那麼，對於任意時刻，我們可以得到該房間的兩個溫度值：你根據生活經驗得到的估計值（系統的預測值）和溫度計的測量值（系統的測量值）。下面我們用這兩個值結合他們各自的雜訊來估算出房間的實際溫度值。假如我們要估算k時刻的實際溫度值。首先你要根據k-1時刻的溫度值，來預測k時刻的溫度。

（1）因為你相信溫度是恆定的，所以你會得到k時刻的溫度預測值是跟k-1時刻一樣的，假設是23度，同時該值的高斯雜訊的偏差是5度。（5是這樣得到的：如果k-1時刻估算出的最優溫度值的偏差是3，你對自己預測的不確定度是4度，他們平方相加再開方，就是5）

（2）然後，你從溫度計那裡得到了k時刻的溫度值，假設是25度，同時該值的偏差是4度
由於我們用於估算k時刻的實際溫度有兩個溫度值，分別是23度和25度。究竟

實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的協方差來判斷。因為Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因為溫度計的協方差比較小（比較相信溫度計），所以估算出的最優溫度值偏向溫度計的值。現在我們已經得到k時刻的最優溫度值了，下一步就是要進入k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什麼自回歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.56度）的偏差。演算法如下：((1-Kg)*5^2)^0.5=2.35。這裡的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入k+1時刻以後k時刻估算出的最優溫度值的偏差（對應於上面的3）。就是這樣，卡爾曼濾波器就不斷的把協方差遞歸，從而估算出最優的溫度值。他運行的很快，而且它只保留了上一時刻的協方差。上面的Kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值。

---

個人理解，用卡爾曼濾波方法做數據融合重要有兩種架構，一是串列架構，二是並行架構。

串列架構，將多個感測器的觀測信息進行依次處理，比如有三個感測器的量測，先用第一個感測器的量測做一次完整的卡爾曼濾波過程；然後再用第二個感測器的量測進行卡爾曼濾波，注意第二次的卡爾曼濾波過程不需要進行一步預測，直接使用第一次完整卡爾曼濾波過程得到的狀態估計結果作為一步預測，然後使用第二個感測器的量測進行測量更新過程，得到第二次卡爾曼濾波過程的最優狀態估計；同樣，這個狀態估計結果將作為第三個感測器量測進行量測更新時的一步預測結果。這個過程與《概率機器人》一書中介紹的EKF定位過程基本是一致的。通過閱讀一些其他文獻，我發現，整個過程中的狀態方程應該是一致的，三個感測器因為量測量可能不同，所以觀測方程可以是不同的。

並行架構，將多個感測器的量測結果，通過適當的方法得到一個統一的量測結果，然後作為觀測輸入，帶入到卡爾曼濾波過程中。這種方法就需要涉及到各個兩側之間的數據關聯過程，而且不同感測器之間的量測數據可能也不一定一致。

至於題主說的電子羅盤和陀螺儀的融合沒接觸過，看過一些gps ins imu作融合的。按照狀態方程物理量的不同現在有直接融合和間接融合兩種方法，直接法將位置等運動狀態作為狀態，然後進行卡爾曼濾波；間接法將誤差作為狀態，進行卡爾曼濾波，然後將估計出的誤差對陀螺儀或電子羅盤的結果進行修正作為最終結果。

沒做過導航方面的，只能說說自己的想法：直接法貌似串列架構更合適，間接法貌似就只能理解為並行架構了，因為作差就相當於已經使用了兩個感測器的數據得到唯一的誤差觀測結果。

---

首先你需要了解一下飛行動力學，卡爾曼濾波是基於模型設計的，不了解模型，光看程序是很難看懂的。

每個項目對象不一樣，你需要按照實際系統進行設計，下面的圖片是一個項目的濾波模型。

系統方程：

觀測方程：

Axm Aym Azm 是加速度計 pm qm rm 是陀螺儀，符號不好打了，省略了。。。基於模型設計，如果是EKF，就把非線性模型線性化，之後離散化。得到相關的濾波矩陣。然後還需要了解感測器的統計特性設置Q R ，然後跑EKF。 濾波器的性能很大程度上取決於模型準不準確（數學模型描述的系統和實際系統一般都會有差別的），matlab可以跑出來，但是在實際設備中濾波器跑一會兒就跑偏了。怎麼保證濾波器的性能的核心演算法都是各個公司，研究所乃至各個國家最核心東西哈（飛行器FDD，組合導航和雷達等的關鍵技術），網上不會隨便公布的。

---

樓主，你好，我現在有加速度計和陀螺儀的靜態數據，怎麼把兩者融合得出，加速度計和與陀螺儀和融合後的曲線三者的圖呢？手頭上只有matlab軟體，還需要買什麼嗎？

---

推薦閱讀：