三、 卡爾曼數據融合

A、 融合效果：本文最后附圖。
B、一行代碼：
a = (1 - d%) * b + d% * c;
a 是 融合輸出 ，b 是預測量，c 是觀察量
這是關鍵的融合代碼。

C、提問：
上述代碼的含義是什么？
加權平均？互補濾波？低通高通濾波器？
同樣的式子，不同的使用過程，有不同的意義。
如果系數是一個固定的值，則有可能是加權平均或者高通低通濾波器。
我講的是動態的系數。獲取數據在變，方差（或協方差）在變，系數就在變。

D、理解：
1、什么情況使用 ？    高斯分布。
單模、零均和、高斯噪聲 （高斯白噪聲）。之前說明了傳感器的噪聲屬于白噪聲。

2、卡爾曼為什么是最佳估計 ？下面使用最小二乘來理解。
推導方式一：
q1 是預測量，q2是觀察量，q是估計值，w是權值。下文來自《自主移動機器人導論》。

其中q1和q2的系數之和為1。
利用這種推導方式可說明卡爾曼是最佳估計，但是并不能推出方差的表達式，并且，權值Wi還是我們自己取的。

推導方式二：
運用貝葉斯定理推導：推出融合表達式和方差表達式

3、卡爾曼的核心。 方差，或者說權重。其實就是卡爾曼增益。

方差公式，跟電阻類似，結果比原來兩者都小。因此就算是預測量的累積誤差較大，也能提高系統的精度，誤差通過迭代而減少。這說明了該方法的有效性。

4、獲得卡爾曼增益的方式對比：
 自動通過統計和計算數據方差 VS 人為經驗估計方差  的對比  ：
 1、自動需要高頻數據，人為經驗不用。
 2、自動具有通用性，人為特定性。
 3、自動計算量大，人為的計算量較少。

從5.35可推出：q = (d%) * q1 + (1-d%) * q2;

5、應用：我們的姿態融合，定位融合都是人為給出卡爾曼增益系數的，叫做“經驗型卡爾曼濾波”。
     經驗之談：
A、參數一般是給的很小，原因跟傳感器（數據）的誤差模型相關；
        參數小的原因：分析卡爾曼增益的式子，預測量的方差較小，觀測量的方差較大，不是同一個數量級，因此增益系數一般給的很小；見定位融合圖，可看出方差情況。
        B、“經驗型卡爾曼濾波”的核心思想是變化卡爾曼增益系數；根據經驗我們能知道哪些條件下傳感器數據的方差大，哪些條件下方差小，從而改變增益系數。
C、融合線程的頻率越高越好。頻率高，一方面收斂快，另外一方面獲得的預測量誤差小。

四、 姿態解算流程

以下是以數據流的方式去理解。
1、傳感器原始數據輸入。
2、傳感器標定，方法很多，比如六面法。把標定后的數據換算成標準單位。
3、確定坐標軸和數據符號，符號用右手定則。
4、歐拉角變換或者四元數變換，變換是為了使陀螺儀角速度的坐標系與加速度計的坐標重合。
5、數據融合，減小方差，降低噪聲。
6、輸出融合后的姿態信息。

五、 DMP
這是MPU6X00芯片的自帶功能。不開源。
性能還不錯，甚至更好。計算有一部分是利用IMU自身的資源，不過依賴芯片產商。

六、數據融合實驗效果：

A、 姿態融合效果：藍色的線是融合輸出。平滑并且響應快。

B、定位融合效果：綠色的線是融合輸出，藍色的線是觀測量，紅色是預測量。

聯系：highspeedlogic

QQ ：1224848052

微信：HuangL1121

郵箱：1224848052@qq.com

網站：http://www.mat7lab.com/

網站：http://www.hslogic.com/

微信掃一掃：