把卡爾曼濾波寫一下吧，思想很簡單匾鸥，不詳細寫了蜡塌，就是根據(jù)方差實現(xiàn)的一種最優(yōu)估計方法。
卡爾曼濾波五個基本的公式
1. 預測階段
1. 1 X(k|k-1)=AX(k-1|k-1)+BU(k)..........先驗估計
A,B為矩陣系數(shù)勿负，X(k|k-1)是根據(jù)上一狀態(tài)預測的結(jié)果馏艾，X(k-1|k-1)是上一次的最優(yōu)結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量奴愉。
1. 2 P(k|k-1)=A P(k-1| k-1) AT+Q..........誤差協(xié)方差
P(k|k-1)是X(k|k-1)的協(xié)方差琅摩，P(k-1| k-1) 是X(k-1|k-1)的協(xié)方差，Q是估計過程的誤差協(xié)方差锭硼。
2. 校正階段
2. 1 Kg(k)= P(k|k-1) HT / (H P(k|k-1) HT + R)..........計算卡爾曼增益
H也為系數(shù)矩陣房资，R為測量值的噪聲協(xié)方差。
2. 2 X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1))..........修正估計
Z(K)是測量值檀头。
2. 3 P(k|k)=（ I-Kg(k) H） P(k|k-1)..........更新誤差協(xié)方差
*I 是為1的矩陣轰异，對于單模型單測量I *為1.
代碼詳解
直接上代碼，看代碼的實現(xiàn)過程要更清晰有邏輯些
先放一個網(wǎng)址鳖擒，解釋的很詳細溉浙。
另外一個卡爾曼濾波九軸融合算法stm32嘗試
以下是一個平衡車的卡爾曼濾波代碼：
float Q_angle=0.001; //陀螺儀噪聲的協(xié)方差
float Q_gyro=0.003; //陀螺儀漂移噪聲的協(xié)方差
float R_angle=0.5; // 加速度計的協(xié)方差
float dt=0.005;
char C_0 = 1;
float Q_bias=0, Angle_err=0; //Q_bias為陀螺儀漂移
float PCt_0=0, PCt_1=0, E=0;
float K_0=0, K_1=0, t_0=0, t_1=0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
void Kalman_Filter(float Gyro,float Accel)
{ //Gyro陀螺儀的測量值，Accel加速度計的角度計算值
Angle+=(Gyro - Q_bias) * dt;
//角度測量模型方程
//就漂移來說認為每次都是相同的Q_bias=Q_bias蒋荚；
//由此得到矩陣
以上代碼對應第一個公式X(k|k-1)=AX(k-1|k-1)+BU(k)
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;
以上代碼對應第二個公式P(k|k-1)=A P(k-1| k-1) AT+Q
公式中Q為向量A的協(xié)方差矩陣
因為漂移噪聲和角度噪聲是相互獨立的戳稽，所以cov(x，y)=0,又因為cov(x,x)=D(x),方差值Q_angle,Q_bias程序開頭定義已給出
卡爾曼濾波的目標就是使P(k-1| k-1)值最小期升，設(shè)為[a,b; c,d] 代入公式計算得
代碼中的
Pdot為矩陣計算得中間變量惊奇，PP對應a,b,c,d
PCt_0 = C_0 * PP[0][0];//矩陣乘法的中間變量
PCt_1 = C_0 * PP[1][0];//C_0=1
//分母
E = R_angle + C_0 * PCt_0;
//增益值
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
以上代碼對應第三個公式Kg(k)= P(k|k-1) HT / (H P(k|k-1)
這里卡爾曼增益是一個二維向量|k0, k1|T，H=|1, 0|,R即為加速度計測量出角度值的噪聲
Angle_err = Accel - Angle;
Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
Gyro_x = Gyro - Q_bias; //計算得角速度值
以上對應第四個公式X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1))播赁，代入相應值即可
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;
}
第五個公式P(k|k)=（ I-Kg(k) H） P(k|k-1)
寫這么多怪有成就感的颂郎，不過排版還是略累啊
