加速度计与陀螺仪互补滤波与卡尔曼滤波核心程序
自己整理的MPU6050中文资料
《自己整理的MPU6050中文资料》一、MPU6050简介MPU6050是一款集成了加速度计和陀螺仪的六轴运动处理传感器。
它采用小巧的封装，具有高精度、低功耗的特点，广泛应用于无人机、智能穿戴设备、智能手机等领域。
通过测量物体在三维空间中的加速度和角速度，MPU6050可以帮助我们实现对运动状态的实时监测和分析。
二、MPU6050核心特点1. 六轴运动处理：MPU6050将加速度计和陀螺仪集成在一个芯片上，实现六轴运动处理。
2. 数字输出：采用数字输出接口，方便与微控制器（如Arduino、STM32等）进行通信。
3. 高精度：加速度计精度为±16g，陀螺仪精度为±2000°/s，满足大多数应用场景的需求。
4. 低功耗：在低功耗模式下，功耗仅为5μA，适用于长时间运行的设备。
5. 小巧封装：采用QFN封装，尺寸仅为4mm×4mm×0.9mm，便于集成到各种产品中。
6. 宽工作电压范围：2.5V至3.5V，适应不同电压需求的场景。
三、MPU6050应用场景1. 无人机：通过MPU6050实时监测飞行器的姿态，实现自主悬停、定高、平稳飞行等功能。
2. 智能穿戴设备：监测用户运动状态，如步数、步频、跌倒检测等，为健康管理提供数据支持。
3. 智能手机：辅机实现重力感应、游戏控制等功能。
4. VR/AR设备：实时监测头部姿态，为虚拟现实体验提供精准的交互。
5. 车载导航：辅助车辆进行姿态检测，提高导航精度。
6. 工业自动化：用于监测设备运行状态，实现故障预警和自动调节。
四、MPU6050接口说明1. SDA：I2C数据线，用于与微控制器通信。
2. SCL：I2C时钟线，与SDA配合实现数据传输。
3. AD0：I2C地址选择线，通过改变AD0的电平，可以设置MPU6050的I2C地址。
4. INT：中断输出，当MPU6050检测到特定事件时，通过INT脚输出中断信号。
基于卡尔曼滤波和互补滤波的AHRS系统研究
本栏目责任编辑：梁书计算机工程应用技术基于卡尔曼滤波和互补滤波的AHRS 系统研究蔡阳，胡杰❋（长江大学计算机科学学院，湖北荆州434023）摘要：AHRS 航姿参考系统中通常需要融合MEMS 传感器数据来进行姿态解算，由于MEMS 传感器自身的一些缺陷导致在姿态解算中会出现较为严重的误差。
AHRS 中常见对加速度计、陀螺仪和磁力计进行卡尔曼滤波、互补滤波的方法，由于使用单一的滤波算法时会出现误差，导致姿态角解算精度不高。
本文采用卡尔曼滤波融合互补滤波的滤波算法，通过卡尔曼滤波对加速度计和陀螺仪起抑制漂移作用，进而得到最优估计姿态角，减小传感器引起的误差，再由估计值和磁力计经过互补滤波滤除噪声，提高姿态角的解算精度。
仿真实验表明：融合滤波算法可以抑制漂移和滤除噪声，在静态和动态条件下，都有良好表现。
关键词：AHRS;MEMS ；姿态解算；卡尔曼滤波；互补滤波中国分类号：TP301文献标识码：A 文章编号：1009-3044(2021)10-0230-03开放科学（资源服务）标识码（OSID ）:Research on AHRS System Based on Kalman Filter and Complementary Filter CAI Yang,HU Jie(School of Computer Science,Yangtze University,Jingzhou 434023,China)Abstract:AHRS heading and attitude reference system usually needs to fuse MEMS sensor data for attitude calculation.Due to some defects of MEMS sensor itself,there will be more serious errors in attitude calculation.Kalman filtering and complementary filtering methods for accelerometers,gyroscopes,and magnetometers are common in AHRS.Due to errors when a single filtering al⁃gorithm is used,the accuracy of the attitude angle calculation is not high.In this paper,the Kalman filter fusion complementary fil⁃ter filter algorithm is used to suppress drift of the accelerometer and gyroscope through Kalman filter,and then obtain the optimal estimated attitude angle,reduce the error caused by the sensor,and then pass the estimated value and the ple⁃mentary filtering filters out noise and improves the accuracy of attitude angle calculation.Simulation experiments show that the fu⁃sion filtering algorithm can suppress drift and filter noise,and it performs well under static and dynamic conditions.Keywords:AHRS;MEMS;attitude calculation;Kalman filter;complementary filter航姿参考系统AHRS(Attitude and Heading Reference Sys⁃tem)由MEMS(Micro-Electro Mechanical System)惯性传感器三轴陀螺仪、三轴加速度计和磁力计的数据融合来进行姿态解算[1]。
陀螺仪卡尔曼滤波算法
陀螺仪卡尔曼滤波算法1. 引言陀螺仪是一种用于测量角速度的传感器，广泛应用于惯性导航、无人机控制、姿态估计等领域。
然而，由于传感器噪声和误差的存在，陀螺仪输出的数据往往不够稳定和准确。
为了解决这个问题，人们提出了许多滤波算法，其中最常用且效果良好的就是卡尔曼滤波算法。
本文将介绍陀螺仪卡尔曼滤波算法的原理、实现过程以及应用场景，并对其优缺点进行讨论。
2. 陀螺仪陀螺仪是一种基于角动量守恒原理工作的传感器。
它通常由一个旋转部件和一个测量部件组成。
旋转部件可以是一个旋转的轴或者一个旋转的盘片，当外界施加力矩时，旋转部件会发生相应的转动。
测量部件通过测量旋转部件的角速度来获取外界施加力矩的信息。
陀螺仪输出的数据通常是角速度，单位为弧度/秒。
然而，由于制造工艺和环境因素的限制，陀螺仪的输出往往存在噪声和误差。
这些噪声和误差会对应用场景中的姿态估计、运动控制等任务产生不利影响。
3. 卡尔曼滤波算法卡尔曼滤波算法是一种递归滤波算法，通过利用系统模型和观测数据，对状态进行估计和预测。
它在估计过程中综合考虑了系统模型的预测值和观测数据的测量值，并通过最小均方误差准则来优化估计结果。
陀螺仪卡尔曼滤波算法主要包括以下几个步骤：3.1 状态空间模型首先，需要建立一个状态空间模型来描述陀螺仪系统。
状态空间模型通常由状态方程和观测方程组成。
状态方程描述了系统的演化规律，可以表示为：x(k) = F * x(k-1) + B * u(k-1) + w(k-1)其中，x(k)表示时刻k的系统状态，F是状态转移矩阵，B是控制输入矩阵，u(k)是控制输入，w(k)是过程噪声。
观测方程描述了系统的输出与状态之间的关系，可以表示为：z(k) = H * x(k) + v(k)其中，z(k)表示时刻k的观测值，H是观测矩阵，v(k)是观测噪声。
3.2 初始化在开始滤波之前，需要对滤波器进行初始化。
通常情况下，可以将初始状态和协方差矩阵设置为零向量和单位矩阵。
互补滤波和卡尔曼滤波代码
互补滤波和卡尔曼滤波代码1互补滤波1.1什么是互补滤波互补滤波是一种传感器融合算法，它将多个传感器的测量值进行整合，得到更加准确的结果。
互补滤波通常用于惯导系统、飞行控制系统、机器人导航等领域，能够有效提高系统的鲁棒性和稳定性。
互补滤波的核心思想是利用不同传感器的优势，从而消除各种噪声和误差。
例如，陀螺仪可以提供高精度的短期姿态测量，但容易出现漂移；加速度计可以提供长期的姿态测量，但响应速度较慢。
因此，互补滤波会将两种传感器的测量值进行加权平均，得到更加准确的姿态估计值。
1.2互补滤波算法流程互补滤波的算法流程包括以下几步：1.读取传感器数据：读取陀螺仪和加速度计的测量值。
2.计算倾角：根据加速度计的测量值计算出当前的姿态角度。
3.计算角速度：根据陀螺仪的测量值计算出当前的角速度。
4.计算互补滤波系数：根据当前角速度和上一次姿态角度，计算出当前的互补滤波系数。
5.更新姿态角度：根据当前的角速度和互补滤波系数，更新姿态角度。
互补滤波系数的计算公式如下：```K=1-alpha*abs(w-w_prev)/a```其中，alpha是一个可调参数，表示互补滤波的衰减速度；w和w_prev分别表示当前和上一时刻的角速度；a是加速度计的测量值，表示重力加速度。
1.3互补滤波的优缺点互补滤波的优点在于可以获得相对准确的姿态估计，而且系统的鲁棒性和稳定性也得到了提高。
此外，互补滤波算法实现简单，对计算资源的需求较低，适合嵌入式系统。
不过，互补滤波的缺点也比较明显。
首先，互补滤波算法对传感器的选择和布局比较敏感，要求每个传感器都能够提供相应的测量值；其次，互补滤波的精度受到其参数的影响，需要进行精细的调整和优化；最后，互补滤波对不同的应用场景需要不同的参数设置，需要进行不断的实验和测试。
2卡尔曼滤波2.1什么是卡尔曼滤波卡尔曼滤波是一种最优估计算法，能够在存在噪声和不确定性的情况下，对系统状态进行估计和控制。
卡尔曼滤波算法最初是为航空航天领域设计的，后来也被广泛应用于汽车导航、机器人控制、金融预测等领域。
MPU6050使用一阶互补和卡尔曼滤波算法平滑角度数据
MPU6050使⽤⼀阶互补和卡尔曼滤波算法平滑⾓度数据最近项⽬上想⽤MPU6050来⾃动探测物体的转向⾓度，花了2天时间学习如何拿陀螺仪的姿态⾓度，发现蛮难的，写点笔记。
下⾯是哔哩哔哩的⼀堆废话讲解，只想看代码本体的可以直接跳到最后。
应⽤场景是51单⽚机环境，有⼀块MPU6060，需要知道硬件板⼦⽔平摆放时，板⼦摆放的姿态和旋转的⾓度。
编译环境只能⽤C语⾔。
⾸先单⽚机通过TTL串⼝接到MPU6050上拿到通信数据，⽔平旋转⾓度需要另外加地磁仪通过南北极磁性拿到。
很遗憾设计硬件时没注意这茬，只⽤了⼀块MPU6050。
不过呢可以⽤旋转时的⾓速度求出旋转幅度（这个下篇说）。
但是拿到原始数据后，发现原始数据的跳动⾮常厉害，需要⽤带滤波的积分算法平滑过滤。
代码演⽰了计算Roll，Pitch⾓和Yaw⾓并⽤卡尔曼过滤算法。
样例⾥⾯有四种芯⽚的演⽰，我⽤的是MPU6050，就直接看这个⽬录了，⾥⾯还有MPU6050+HMC5883L磁⼒计的样式，可惜⼿头板⼦当初没有想过算Yaw⾓旋转要磁⼒计，也就莫法实现读取Yaw⾓。
加HMC5883L磁⼒计的那个样例，是读的磁⼒计的数据来算Yaw轴⾓度。
MPU6050的重⼒加速度出来的z轴数据基本不咋变化，仅依靠x和y轴数据肯定不准的，所以这⾥通过重⼒加速度算出来Yaw轴的⾓度⽆意义。
继续回来说代码。
下载回来的样例代码扩展名是.ino，搞不懂啥，改成.c，⼀样看，c语⾔万岁！⾸先要先拿到陀螺仪的数据，⾓速度和重⼒加速度。
如何读取MPU6050的数据我略过。
⽹上有很多现成的样例，直接拿来⽤。
/* IMU Data */float accX, accY, accZ;float gyroX, gyroY, gyroZ;accX = ((i2cData[0] << 8) | i2cData[1]);accY = ((i2cData[2] << 8) | i2cData[3]);accZ = ((i2cData[4] << 8) | i2cData[5]);//tempRaw = (i2cData[6] << 8) | i2cData[7];gyroX = (i2cData[8] << 8) | i2cData[9];gyroY = (i2cData[10] << 8) | i2cData[11];gyroZ = (i2cData[12] << 8) | i2cData[13];i2cData是MPU6050读到的14个字节的数据。
卡尔曼滤波算法的程序实现和推导过程
卡尔曼滤波算法的程序实现和推导过程---蒋海林（QQ：280586940）---卡尔曼滤波算法由匈牙利裔美国数学家鲁道夫·卡尔曼（Rudolf Emil Kalman）创立，这个数学家特么牛逼，1930年出生，现在还能走能跳，吃啥啥麻麻香，但他的卡尔曼滤波算法已经广泛应用在航空航天，导弹发射，卫星在轨运行等很多高大上的应用中。
让我们一边膜拜一边上菜吧，下面就是卡尔曼滤波算法的经典程序，说是经典，因为能正常运行的程序都长得差不多，在此向原作者致敬。
看得懂的，帮我纠正文中的错误；不太懂的，也不要急，让我慢慢道来。
最后希望广大朋友转载时，能够保留我的联系方式，一则方便后续讨论共同进步，二则支持奉献支持正能量。
void Kalman_Filter(float Gyro,float Accel)///角速度，加速度{///陀螺仪积分角度（先验估计）Angle_Final = Angle_Final + (Gyro - Q_bias) * dt;///先验估计误差协方差的微分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;///计算角度偏差Angle_err = Accel - Angle_Final;///卡尔曼增益计算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_Final += K_0 * Angle_err; ///后验估计最优角度值Q_bias += K_1 * Angle_err; ///更新最优估计值的偏差Gyro_Final = Gyro - Q_bias; ///更新最优角速度值}我们先把卡尔曼滤波的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个方程和上面的程序对应起来。
加速度计与陀螺仪融合一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序
一阶互补// a=tau / (tau + loop time)// newAngle = angle measured with atan2 using the accelerometer//加速度传感器输出值// newRate = angle measured using the gyro// looptime = loop time in millis()float tau=0.075;float a=0.0;float Complementary(float newAngle,float newRate,int looptime) {float dtC =float(looptime)/1000.0;a=tau/(tau+dtC);//以下代码更改成白色，下载后恢复成其他颜色即可看到x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);return x_ang leC;}二阶互补// newAngle = angle measured with atan2 using the accelerometer// newRate = angle measured using the gyro// looptime = loop time in millis()float Complementary2(float newAngle,float newRate,int looptime) {float k=10;float dtc2=float(looptime)/1000.0;//以下代码更改成白色，下载后恢复成其他颜色即可看到x1 =(newAng le - x_angle2C)*k*k;y1 = dtc2*x1 + y1;x2 = y1 + (newAngle - x_angle2C)*2*k + newRate;x_angle2C =dtc2*x2 + x_angle2C;return x_angle2C;} Here too we just have to set the k and magically we get the angle.卡尔曼滤波// KasBot V1 - Kalman filter modulefloat Q_angle =0.01;//0.001float Q_gyro =0.0003;//0.003float R_angle =0.01;//0.03float x_bias =0;float P_00 =0, P_01 =0, P_10 =0, P_11 =0;float y, S;float K_0, K_1;// newAngle = angle measured with atan2 using the accelerometer // newRate = angle measured using the gyro// looptime = loop time in millis()float kalmanCalculate(float newAngle,float newRate,int looptime) {float dt =float(looptime)/1000;x_angle += dt *(newRate - x_bias);//以下代码更改成白色，下载后恢复成其他颜色即可看到P_00 +=- dt * (P_10 + P_01) + Q_angle * dt;P_01 +=- dt * P_11;P_10 +=- dt * P_11;P_11 +=+ Q_gyro * dt;y = newAngle - x_angle;S = P_00 + R_angle;K_0 = P_00 / S;K_1 = P_10 / S;x_angle +=K_0 * y;x_bias +=K_1 * y;P_00 -= K_0 * P_00;P_01 -= K_0 * P_01;P_10 -= K_1 * P_00;P_11 -= K_1 * P_01;return x_angle;} To get the answer, you have to set 3 parameters: Q_angle, R_angle,R_gyro.详细卡尔曼滤波/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:* $Id: tilt.c,v 1.1 2003/07/09 18:23:29 john Exp $** 1 dimensional tilt sensor using a dual axis accelerometer* and single axis angular rate gyro. The two sensors are fused* via a two state Kalman filter, with one state being the angle* and the other state being the gyro bias.* Gyro bias is automatically tracked by the filter. This seems* like magic.** Please note that there are lots of comments in the functions and* in blocks before the functions. Kalman filtering is an already complex* subject, made even more so by extensive hand optimizations to the C code* that implements the filter. I've tried to make an effort of explaining* the optimizations, but feel free to send mail to the mailing list,* autopilot-devel@, with questions about this code.*** (c) 2003 Trammell Hudson <hudson@>**************** This file is part of the autopilot onboard code package.** Autopilot is free software; you can redistribute it and/or modify* it under the terms of the GNU General Public License as published by* the Free Software Foundation; either version 2 of the License, or* (at your option) any later version.** Autopilot is distributed in the hope that it will be useful,* but WITHOUT ANY W ARRANTY; without even the implied warranty of* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details.** You should have received a copy of the GNU General Public License* along with Autopilot; if not, write to the Free Software* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA **/#include <math.h>/** Our update rate. This is how often our state is updated with* gyro rate measurements. For now, we do it every time an* 8 bit counter running at CLK/1024 expires. You will have to* change this value if you update at a different rate.*/static const float dt = ( 1024.0 * 256.0 ) / 8000000.0;/** Our covariance matrix. This is updated at every time step to* determine how well the sensors are tracking the actual state.*/static float P[2][2] = {{ 1, 0 },{ 0, 1 },};/** Our two states, the angle and the gyro bias. As a byproduct of computing * the angle, we also have an unbiased angular rate available. These are* read-only to the user of the module.*/float angle;float q_bias;float rate;/** R represents the measurement covariance noise. In this case,* it is a 1x1 matrix that says that we expect 0.3 rad jitter* from the accelerometer.*/static const float R_angle = 0.3;/** Q is a 2x2 matrix that represents the process covariance noise.* In this case, it indicates how much we trust the acceleromter* relative to the gyros.*/static const float Q_angle = 0.001;static const float Q_gyro = 0.003;/** state_update is called every dt with a biased gyro measurement* by the user of the module. It updates the current angle and* rate estimate.** The pitch gyro measurement should be scaled into real units, but* does not need any bias removal. The filter will track the bias.** Our state vector is:** X = [ angle, gyro_bias ]** It runs the state estimation forward via the state functions:** Xdot = [ angle_dot, gyro_bias_dot ]** angle_dot = gyro - gyro_bias* gyro_bias_dot = 0** And updates the covariance matrix via the function:** Pdot = A*P + P*A' + Q** A is the Jacobian of Xdot with respect to the states:** A = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ]* [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]** = [ 0 -1 ]* [ 0 0 ]** Due to the small CPU available on the microcontroller, we've* hand optimized the C code to only compute the terms that are* explicitly non-zero, as well as expanded out the matrix math* to be done in as few steps as possible. This does make it harder* to read, debug and extend, but also allows us to do this with* very little CPU time.*/voidstate_update( const float q_m /* Pitch gyro measurement */) {/* Unbias our gyro */const float q = q_m - q_bias;/** Compute the derivative of the covariance matrix** Pdot = A*P + P*A' + Q** We've hand computed the expansion of A = [ 0 -1, 0 0 ] multiplied* by P and P multiplied by A' = [ 0 0, -1, 0 ]. This is then added * to the diagonal elements of Q, which are Q_angle and Q_gyro.*/const float Pdot[2 * 2] = {Q_angle - P[0][1] - P[1][0], /* 0,0 */- P[1][1], /* 0,1 */- P[1][1], /* 1,0 */Q_gyro /* 1,1 */};/* Store our unbias gyro estimate */rate = q;/** Update our angle estimate* angle += angle_dot * dt* += (gyro - gyro_bias) * dt* += q * dt*/angle += q * dt;/* Update the covariance matrix */P[0][0] += Pdot[0] * dt;P[0][1] += Pdot[1] * dt;P[1][0] += Pdot[2] * dt;P[1][1] += Pdot[3] * dt;}/** kalman_update is called by a user of the module when a new* accelerometer measurement is available. ax_m and az_m do not * need to be scaled into actual units, but must be zeroed and have* the same scale.** This does not need to be called every time step, but can be if* the accelerometer data are available at the same rate as the* rate gyro measurement.** For a two-axis accelerometer mounted perpendicular to the rotation * axis, we can compute the angle for the full 360 degree rotation* with no linearization errors by using the arctangent of the two* readings.** As commented in state_update, the math here is simplified to* make it possible to execute on a small microcontroller with no* floating point unit. It will be hard to read the actual code and* see what is happening, which is why there is this extensive* comment block.** The C matrix is a 1x2 (measurements x states) matrix that* is the Jacobian matrix of the measurement value with respect* to the states. In this case, C is:** C = [ d(angle_m)/d(angle) d(angle_m)/d(gyro_bias) ]* = [ 1 0 ]** because the angle measurement directly corresponds to the angle* estimate and the angle measurement has no relation to the gyro* bias.*/voidkalman_update(const float ax_m, /* X acceleration */const float az_m /* Z acceleration */){/* Compute our measured angle and the error in our estimate *///以下代码更改成白色，下载后恢复成其他颜色即可看到const float angle_m = atan2( -az_m, ax_m );const float angle_err = angle_m - angle;/** C_0 shows how the state measurement directly relates to* the state estimate.** The C_1 shows that the state measurement does not relate* to the gyro bias estimate. We don't actually use this, so* we comment it out.*/const float C_0 = 1;/* const float C_1 = 0; *//** PCt<2,1> = P<2,2> * C'<2,1>, which we use twice. This makes * it worthwhile to precompute and store the two values.* Note that C[0,1] = C_1 is zero, so we do not compute that* term.*/const float PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */ const float PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 *//** Compute the error estimate. From the Kalman filter paper:** E = C P C' + R** Dimensionally,** E<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1>** Again, note that C_1 is zero, so we do not compute the term.*/const float E =R_angle+ C_0 * PCt_0/* + C_1 * PCt_1 = 0 */;/** Compute the Kalman filter gains. From the Kalman paper:** K = P C' inv(E)** Dimensionally:** K<2,1> = P<2,2> C'<2,1> inv(E)<1,1>** Luckilly, E is <1,1>, so the inverse of E is just 1/E.*/const float K_0 = PCt_0 / E;const float K_1 = PCt_1 / E;/** Update covariance matrix. Again, from the Kalman filter paper: ** P = P - K C P** Dimensionally:** P<2,2> -= K<2,1> C<1,2> P<2,2>** We first compute t<1,2> = C P. Note that:** t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0]** But, since C_1 is zero, we have:** t[0,0] = C[0,0] * P[0,0] = PCt[0,0]** This saves us a floating point multiply.*/const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */ const float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 */P[0][0] -= K_0 * t_0;P[0][1] -= K_0 * t_1;P[1][0] -= K_1 * t_0;P[1][1] -= K_1 * t_1;/** Update our state estimate. Again, from the Kalman paper:** X += K * err** And, dimensionally,** X<2> = X<2> + K<2,1> * err<1,1>** err is a measurement of the difference in the measured state* and the estimate state. In our case, it is just the difference* between the two accelerometer measured angle and our estimated * angle.*/angle += K_0 * angle_err;q_bias += K_1 * angle_err;}。
卡尔曼滤波程序
/***************************************************************************//* kalman.c *//* 1-D 卡尔曼滤波算法, 通过加速度计（倾角）和陀螺仪完成*//* Author: Rich Chi Ooi *//* Version: 1.0 *//* Date:30.05.2003 *//* 改编自Trammel Hudson(hudson@) *//* ------------------------------- *//* 程序编译: *//* Linux */ /* gcc68 -c XXXXXX.c (to create object file) *//* gcc68 -o XXXXXX.hex XXXXXX.o ppwa.o */ /* Upload data : *//* ul filename.txt *//***************************************************************************/ /* 本版内容: *//***************************************************************************/ /* This is a free software; you can redistribute it and/or modify *//* it under the terms of the GNU General Public License as published *//* by the Free Software Foundation; either version 2 of the License, *//* or (at your option) any later version. *//* */ /* this code is distributed in the hope that it will be useful, *//* but WITHOUT ANY W ARRANTY; without even the implied warranty of *//* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *//* GNU General Public License for more details. *//* */ /* You should have received a copy of the GNU General Public License *//* along with Autopilot; if not, write to the Free Software *//* Foundation, Inc., 59 Temple Place, Suite 330, Boston, *//* MA 02111-1307 USA */ /***************************************************************************/ #include <math.h>#include "eyebot.h"/** 陀螺仪采样周期20ms* 更新频率不同的话可以改变*/static const float dt = 0.02; //陀螺仪采样周期/** 协方差矩阵.每次更新决定传感器跟踪实际状态的好坏*/static float P[2][2] = {{ 1, 0 }, //协方差矩阵{ 0, 1 }};/** 两个状态，角度和陀螺仪偏差.作为计算角度和偏差的副产品，我们可以得到无偏的角速率* 以下是该模块对于作者的只读量*/float angle;float q_bias;float rate; //3个无偏输出量/** R为测量协方差noise.R=E[vvT]* 这种情况下为1×1矩阵* 0.1 rad jitter from the inclinometer.* for a 1x1 matrix in this case v = 0.1*/static const float R_angle = 0.001 ;/** Q是2×2过程协方差矩阵噪声.* 表示对加速度计和陀螺仪的信任程度*/static const float Q_angle = 0.001;static const float Q_gyro = 0.0015;/** 状态每次通过带有偏移量量的陀螺仪测量值进行更新。