卡尔曼滤波器 MPU6050.pptVIP

基于MPU6050及卡尔曼滤波的角度测量 MPU6050的基本应用 卡尔曼滤波工作原理及应用 MPU6050 ▲MPU-6050 ：是6轴运动处理传感器，集成了3轴陀螺仪传感器和3轴加速度传感器； ▲MPU6050共有117个寄存器，设计中主要运用到的寄存器有： 1、分频寄存器 0x19 2、配置寄存器 0x1A 3、陀螺仪配置寄存器 0x1B 4、加速度计配置寄存器 0x1C 5、加速度计数据寄存器 0x3B—0x40 6、陀螺仪数据寄存器 0x43—0x48 7、电源管理寄存器1 0x6B 8、我是谁寄存器 0x75 用于标识设备的身份 ▲MPU6050的函数初始化：解除休眠（0x00）——分频设置（0x07） ——寄存器配置（0x06）——陀螺仪配置（0x18） ——加速度计配置（0x01） ▲数据处理：……… I2C总线通信 ▲应用：四旋翼飞行器、平衡小车、行人导航仪、手持型游戏产品、3D遥控器。 内部工作 卡尔曼滤波器 ◆卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值 ——是一种收敛性 ◆应用：机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪 、计算机图像处理（头脸识别、图像分割） Kalman 概念：卡尔曼滤波器是最优化自回归数据处理算法。 一、符号说明： K：系统的现在状态；Kg：卡尔曼增益 二、核心内容：5条公式 首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述： X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值： Z(k)=H X(k)+V(k)  W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance（协方差） 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。 、 基于系统的上一状态而预测出现在状态： X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)式(1)中，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。我们用P表示covariance： P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2) 现在状态(k)的最优化估算值X(k|k)： X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) 其中Kg为卡尔曼增益(Kalman Gain)： Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance： P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。 设计中的总体思路图