卡爾曼濾波配合程式講解

#define PI 3.14159
//角度參數
float Angle, Angle_dot;   //小車最終傾斜角度、角速度
float Angle_aYZ;          //由Y軸Z軸上的加速度傳感器測得的數值，計算出傾斜角度
float Angle_gX;           //由X軸的陀螺儀傳感器測得的數值，計算出角速度
//卡爾曼參數
float  Q_angle = 0.01; //陀螺儀噪聲的協方差
float  Q_gyro = 0.01;     //陀螺儀漂移噪聲的協方差
float  R_angle = 0.003;           // 加速度計的協方差
float  dt = 0.005;         //dt為kalman濾波器採樣時間;
char   C_0 = 1;
float  Q_bias, Angle_err;   //Q_bias為陀螺儀漂移
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; //輸出值(後驗估計)的微分=角速度
}

//傾角計算（卡爾曼融合）
void Angle_Calcu(void)
{
    //------根據加速度分量測得角速度--------------------------
    //不自測，加速度傳感器範圍設置  0  ±2g     16384 LSB/g
    Angle_aYZ = atan2((Out_Data(ACCEL_YOUT_H) - 300),
                      (Out_Data(ACCEL_ZOUT_H) - (16384 - 16450))) * 180 / PI
                ; //去除零點偏移,計算得到角度（弧度）
    //弧度轉換為度,

    //-------角速度-------------------------
    //不自測，陀螺儀測量範圍設置  0  ±250°/s  131LSB/(°/s)   0.00763358 (°/s)/LSB
    Angle_gX = (Out_Data(GYRO_XOUT_H) - 0) * 0.00763358
               ; //0為補償量，在靜止是測得的角速度為0LSB；

    //-------卡爾曼濾波融合-----------------------
    Kalman_Filter(Angle_aYZ - 0,
                  Angle_gX - 0);       //卡爾曼濾波計算傾角,減去零點偏移，
}

首先是卡爾曼濾波的5個方程

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)//先驗估計 
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)//協方差矩陣的預測 
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (3)//計算卡爾曼增益 
X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通過卡爾曼增益進行修正 
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)//跟新協方差陣

卡爾曼濾波的5個方程

驗估計
協方差矩陣預測
計算卡爾曼增益
通過卡爾曼增益進行修整
根新協方差陣

5個式子比較抽象，現在直接用實例來說

—，對於角度來說，我們認為此時的角度可以近似認為是上一時刻的角度值加上上一時刻陀螺儀測得的角加速度值乘以時間，因為，角度微分等於時間的微分乘以角速度。

但是陀螺儀有個靜態漂移（而且還是變化的），靜態漂移就是靜止了沒有角速度然後陀螺儀也會輸出一個值，這個值肯定是沒有意義的，計算時要把它減去。由此我們得到了當前角度的預測值 Angle

Angle=Angle+(Gyro - Q_bias) * dt;

其中等號左邊Angle為此時的角度，等號右邊Angle為上一時刻的角度，Gyro 為陀螺儀測的角速度的值，dt是兩次濾波之間的時間間隔。

float  dt=0.005;                      這是程序中的定義

同時 Q_bias也是一個變化的量。

但是就預測來說認為現在的漂移跟上一時刻是相同的即

Q_bias=Q_bias

將兩個式子寫成矩陣的形式

得到上式，這個式子對應於卡爾曼濾波的第一個式子

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)//先驗估計

X(k|k-1)為2維列向量，A為2維方陣，X(k-1|k-1)為2維列向量，B 為2維列向量，U(k) 為Gyro

二，這裡是卡爾曼濾波的第二個式子

接著是預測方差陣的預測值，這裡首先要給出兩個值，一個是漂移的噪聲，一個是角度值的噪聲，（所謂噪聲就是數據的方差值）

P(k|k-1)=A P(k-1|k-1) A’+Q

這裡的Q為向量的協方差矩陣，即
因為漂移噪聲還有角度噪聲是相互獨立的，則 =0； =0 又由性質可知cov（x，x）=D（x）即方差，所以得到的矩陣如下，這裡的兩個方差值是開始就給出的常數程序中的定義如下

float  Q_angle=0.001;   
float  Q_gyro=0.003;

接著是這一部分A P(k-1|k-1) A’,其中的（P（k-1）|P(k-1)）為上一時刻的預測方差陣卡爾曼濾波的目標就是要讓這個預測方差陣最小。其中P(k-1|k-1)設為 ,第一式已知A為
則計算A P(k-1|k-1) A’+Q（就是個矩陣乘法和加法，算算吧）結果如下

很小為了計算簡便忽略不計。於是得到

a,b,c,d分別和矩陣的P[0][0],P[0][1],P[1][0],P[1][1] 計算過程轉化為如下程序，代換即可

Pdot[0]=Q_angle - PP[0][1] - PP[1][0];  
Pdot[1]= - PP[1][1]; 
Pdot[2]= - PP[1][1];/ 
Pdot[3]=Q_gyro; 

PP[0][0] += Pdot[0] * dt;    
PP[0][1] += Pdot[1] * dt;  
PP[1][0] += Pdot[2] * dt; 
PP[1][1] += Pdot[3] * dt;

三，這裡是卡爾曼濾波的第三個式子

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (3)//計算卡爾曼增益

即計算卡爾曼增益，這是個二維向量設為，這裡的 = 為由此kg= P(K|K-1)+R，這裡又有一個常數R，程序中的定義如下 float R_angle=0.5; 這個指的是角度測量噪聲值，則式子的分母=P[0][0]+R_angle 即程序中的

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;

四，用誤差還有卡爾曼增益來修正

X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通過卡爾曼增益進行修正

這個矩陣帶進去就行了Z（k)=Accel.....注意這個是加速度計算出來的角度

Angle_err = Accel - Angle;

對應程序如下

Angle    += K_0 * Angle_err; 
Q_bias    += K_1 * Angle_err;

同時為了PID控制還有下次的使用把角速度算出來了

Gyro_x   = Gyro - Q_bias;

五，最後一步對矩陣P進行更新，因為下一次濾波時要用到

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;

P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)//跟預測方差陣

這個很簡單，矩陣帶進去算就行了

六，總結

卡爾曼濾波一共只需要給很少的初始值量，

float  Q_angle=0.001;   
float  Q_gyro=0.003; 
float  R_angle=0.5;

以及系統的初始量angle還有Q_bias 還有預測誤差矩陣P，程序裡給的是0（數組）

理論上由於卡爾曼濾波是迭代的算法，當時間充分長以後。濾波估值將與初始值的選取無關。

但是實際上並不是如此，比如測量方差值一直在變化。

其實卡爾曼的程序相當的簡單，只要你理解了他的那5條公式。

卡爾曼濾波 配合程式 講解