找回密码
 立即注册

QQ登录

只需一步,快速开始

扫一扫,访问微社区

查看: 33996|回复: 925

加速度计与陀螺仪融合一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序

  [复制链接]
发表于 2015-3-26 15:21:19 | 显示全部楼层 |阅读模式
本帖最后由 Bluesky 于 2018-11-6 17:00 编辑

[C] 纯文本查看 复制代码
//一阶互补 
// 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_angleC; 
}

//二阶互补 
// 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 = (newAngle -  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 module  
float Q_angle  =  0.01; //0.001                         // 角度数据置信度
float Q_gyro   =  0.0003;  //0.003                         // 角速度数据置信度
float R_angle  =  0.01;  //0.03                          // 方差噪声
float x_bias = 0;         //陀螺仪的偏差
float rate;                        //去除偏差后的角速度
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; //过程协方差矩阵P[2][2]
float angle_err;        //角度偏量
float S;         //计算的过程量
float K_0;        //含有卡尔曼增益的另外一个函数,用于计算最优估计值
float 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;                 //卡尔曼滤波采样频率
        rate = newRate - x_bias;
        x_angle += dt * rate;                                         //角速度积分得出角度,先验估计
        P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt; //先计算过程协方差的微分矩阵,再对dt积分
        P_01 +=  - dt * P_11;
        P_10 +=  - dt * P_11; 
        P_11 +=  + Q_gyro * dt;                                  //先验估计误差协方差
        
        angle_err = newAngle - x_angle;         
        K_0 = P_00 / S;                                                 //计算角度偏差
        S = P_00 + R_angle;        //计算卡尔曼增益
        K_1 = P_10 / S;  
        
        x_angle +=  K_0 * angle_err;                                         //给出角度最优估计值
        x_bias  +=  K_1 * angle_err;                                         //更新最优估计值误差
        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.
回复

使用道具 举报

发表于 2015-4-27 15:35:55 | 显示全部楼层
多谢分享,回复看看!
回复 支持 1 反对 0

使用道具 举报

发表于 2015-4-28 20:43:15 | 显示全部楼层
看看,顶一个
回复 支持 反对

使用道具 举报

发表于 2015-4-29 14:38:49 | 显示全部楼层
感谢楼主分享
回复 支持 反对

使用道具 举报

发表于 2015-4-29 14:44:37 | 显示全部楼层
感谢楼主分享。。
回复 支持 反对

使用道具 举报

发表于 2015-5-18 17:35:21 | 显示全部楼层
能写详细一点心得吗
回复 支持 反对

使用道具 举报

发表于 2015-5-21 13:05:20 | 显示全部楼层
感谢楼主分享~!!~
回复 支持 反对

使用道具 举报

发表于 2015-5-21 15:35:46 | 显示全部楼层
好东西,支持
回复 支持 反对

使用道具 举报

发表于 2015-5-22 23:20:30 | 显示全部楼层
感谢呀 正是需要的时候
回复 支持 反对

使用道具 举报

发表于 2015-5-26 22:16:28 | 显示全部楼层
不错,表示对楼主支持。
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

快速回复 返回顶部 返回列表