MPU6050卡尔曼滤波算法推导过程
2星 需积分: 40 18 浏览量
2018-12-26
15:16:53
上传
评论 15
收藏 312KB PDF 举报 
陀螺仪卡尔曼滤波算法和推导过程
X(K)=AX(K-1)+BU(K-1)+W(K-1)
Z(K)=HX(K)+V(K)
说明，下面带 T 的表示转置。
卡尔曼滤波的黄金五条公式
X(k|k-1)=AX(k-1|k-1)+BU(k)……….先验估计
P(k|k-1)=A P(k-1| k-1) AT+Q……….误差协方差
Kg(k)= P(k|k-1) HT / (H P(k|k-1) HT + R)……….计算卡尔曼增益
X(k|k)= X(k|k-1) + Kg(k)(Z(k) - H X(k|k-1))……….修正估计
P(k|k)=（ I-Kg(k) H） P(k|k-1)……….更新误差协方差
下面的程序主要针对 MPU6050 的姿态角的滤波。
float Q_angle=0.001; //陀螺仪噪声的协方差
float Q_gyro=0.003; //陀螺仪漂移噪声的协方差
float R_angle=0.5; // 加速度计的协方差
float dt=0.005;
float Q_bias=0, Angle_err=0; //Q_bias 为陀螺仪漂移
float E=0;
float K_0=0, K_1=0,;//K_0 为角度增益，K_1 为漂移 Q_bias 增益
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
首先建立的是过程方程，这里的状态变量是 angle 以及 Q_bias,角度以及陀螺仪的漂移。
那么已经建立了这里的预测方程，没有加上噪声。
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)
这里再分析第二条公式，P(k|k-1)=A P(k-1| k-1) AT+Q。可以在之前看出，A=[1,-dt;0,1]。而 Q
评论1
最新资源