MPU6050卡尔曼滤波算法推导过程_卡尔曼滤波器五个公式 - CSDN文库

卡尔曼滤波

2星需积分: 40 84 浏览量更新于2023-03-16 评论 15 收藏 312KB PDF 举报

身份认证购VIP最低享 7 折!

领优惠券(最高得80元）

资源详情

资源评论

资源推荐

陀螺仪卡尔曼滤波算法和推导过程

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

本内容试读结束，登录后可阅读更多

下载后可阅读完整内容，剩余3页未读，立即下载

评论1

大头熊在学习

2019-07-15

MPU6050卡尔曼滤波算法推导过程简单

为谁唱歌

粉丝: 11
资源: 31

会员权益专享

图片转文字

全年可省5，000元立即开通

最新资源

资源上传下载、课程学习等过程中有任何疑问或建议，欢迎提出宝贵意见哦~我们会及时处理！点击此处反馈