ICM20602互补滤波

实在滤不出数据，直接用原始数据即 fDeltaValue = 0;//不补偿


#define   CONSTANT      2     //补偿系数  减小要适当增大D  （取1到4）
#define   DT          0.044 //积分系数      决定了跟踪速度 0.044
#define   R_Gyro      0.0265  //角速度系数   0.0265
#define   R_Acc       0.046  //角度系数  0.046float Angle;
float  AngleIntegral;void Complementary_Filter(void){float  fDeltaValue = 0;                              //补偿量static uint8 first_angle=0;//角速度（消除零漂）ICM_Real.gyro[2] = (ICM_Offset.gyro[2] - icm_gyro_z)*R_Gyro; //加速度（消除零漂）ICM_Real.acc[1] =  (icm_acc_y-  ICM_Offset.acc[1] )*R_Acc;if(!first_angle)//第一次进入初始化{first_angle=1;AngleIntegral=ICM_Real.acc[1];}//最终融合角度Angle = AngleIntegral;  //时间系数矫正、补偿量fDeltaValue = ( ICM_Real.acc[1] - Angle) / CONSTANT;    fDeltaValue = 0;//不补偿//角速度积分融合后的角度AngleIntegral += (ICM_Real.gyro[2] + fDeltaValue) * DT; }

滤波代码2

float acc_ratio = 1.2;      //加速度计比例
float gyro_ratio = 4.2;    //陀螺仪比例
#define PI 3.141596
#define dt 0.01  //采样周期 陀螺仪角速度积分系数,增长缓慢就增加float angle_calc(float angle_m, float gyro_m)
{      float temp_angle;               float gyro_now;    float error_angle; static float last_angle;    static uint8 first_angle;  if(!first_angle)//判断是否为第一次运行本函数    {    //如果是第一次运行，则将上次角度值设置为与加速度值一致    first_angle = 1;    last_angle = angle_m;    }    //根据测量到的加速度值转换为角度之后与上次的角度值求偏差    gyro_now = -gyro_m * gyro_ratio;  //根据偏差与陀螺仪测量得到的角度值计算当前角度值    error_angle = (angle_m - last_angle)*acc_ratio;  //保存当前角度值    temp_angle = last_angle + (error_angle + gyro_now)*dt;      last_angle = temp_angle;  return temp_angle;
}

ICM20602互补滤波相关推荐

对比直立车模控制中的互补滤波、Karlman滤波和参考滤波方案
前几天,有参加智能车竞赛的同学在公众号上询问一个关于直立车模控制中角度融合算法的问题.感觉这是所有初次准备智能车模竞赛的同学都会碰到的问题. 卓大大,请问您可以针对卡尔曼滤波.互补滤波.清华滤波的原理 ...
四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
【51单片机快速入门指南】4.3.2： MPU6050：一阶互补滤波、二阶互补滤波和卡尔曼滤波获取欧拉角
目录源码 MPU6050_Filter.c MPU6050_Filter.h 使用方法测试程序一阶互补滤波效果二阶互补滤波效果卡尔曼滤波效果总结普中51-单核-A2 STC89C5 ...
STC51单片机15——MPU6050六轴数据融合，互补滤波，时间常数可调，可稳定运行，串口显示角度值
51单片机用模拟IIC的方式读取MPU6050的原始数据,之后经过换算转成三轴加速度和三轴角速度.设定定时器,以固定的频率采集以上得到的数据,并加入互补滤波,去除加速度的噪声以及陀螺仪的零飘.注意,本 ...
陀螺仪与磁力计的姿态融合——互补滤波(续)
大家好,我是学电子的小白白~ 上一篇我们讲了陀螺仪与加速度计的数据融合,本篇文章是互补滤波的续篇,接着讲陀螺仪与磁力计的数据融合. 1)为什么需要磁力计的数据先回忆一下上一篇内容,加速度计是如何修正 ...
MPU6050加速度、角速度的解算以及互补滤波使用
MPU6050加速度.角速度的解算以及互补滤波使用 MPU6050加速度的解算 MPU6050角速度的解算互补滤波使用(数据融合) 数字低通滤波器数字高通滤波器 MPU6050加速度的解算先根据 ...
树莓派pico mpu6050 一阶互补滤波四元数法解算姿态角
micro-python:一阶互补滤波&四元数法代码 2.系统方案 2.1.组成本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分电池为一节14500锂电池, 容 ...
姿态解算进阶：互补滤波（陀螺仪、加速度计、地磁计数据融合）
互补滤波原理: 在四轴入门理论知识那节我们说,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系.陀 ...
基于RflySim平台的mahony(含磁罗盘)互补滤波在pixhawk仿真及实物实验（带实验数据）
写在前面本案例实验采用RflySim平台,该平台可以高效快速编写代码,使用simulink模型搭建,可以见代码直接生成对应的C代码,并一键将代码烧录Pixhawk中,是一种快速开发平台,RflySi ...
四轴之互补滤波与四元数算法简单分析
有人问我关于四元数姿态解算算法的分析,每次都解释好久,今日空闲,特发一帖,供大家参考.本分析将结合程序,分析姿态解算思路,由于能力有限,难免有错误之处请谅解,同时希望能够抛砖引玉,得到大神指点.感谢圆 ...

ICM20602互补滤波

ICM20602互补滤波相关推荐

最新文章

热门文章