NXP MCU 技术论坛
登录
直播中
余义
10年用户
11经验值
擅长:可编程逻辑 电源/新能源 MEMS/传感技术 测量仪表 嵌入式技术 制造/封装 模拟技术 连接器 EMC/EMI设计 光电显示 存储技术 EDA/IC设计 处理器/DSP 接口/总线/驱动 控制/MCU RF/无线
私信
关注
[资料]
直立车用到的三种融合,在这里都有
Angle
/******************************三种滤波函数,每种都调试下,比较动态和静态过程****************************************************/
/*******************下面滤波过程很重要,很重要,很重要,重要的事情说三遍*******************************************************/
/*******************下面滤波过程很重要,很重要,很重要,重要的事情说三遍*******************************************************/
float Angle,Gyro_y; //*************
float Q_angle=0.001;//0.001 //*************
float Q_gyro=0.003;//0.03 //*************
float R_angle=0.5;//0.5 //*************
//float dt=0.01;//0.1 //dt为kalman滤波器采样时间; //*************
char C_0 = 1; //*************
float Q_bias, Angle_err; //*************
float PCt_0, PCt_1, E; //*************
float K_0, K_1, t_0, t_1; //*************
float Pdot[4] ={0,0,0,0}; //*************
float PP[2][2] = { { 1, 0 },{ 0, 1 } }; //*************
float w_kp=1;//0.945 //*************
/*矩阵卡尔曼函数*/ //*************
float Kalman_Filter(float Accel,float Gyro) //*************
{ //*************
Angle+=(Gyro - Q_bias) * dt; //先验估计 //*************
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分 //*************
Pdot[1]=- PP[1][1]; //*************
Pdot[2]=- PP[1][1]; //*************
Pdot[3]=Q_gyro; //*************
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分 //*************
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差 //*************
PP[1][0] += Pdot[2] * dt; //*************
PP[1][1] += Pdot[3] * dt; //*************
Angle_err = Accel - Angle; //zk-先验估计 //*************
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 += K_0 * Angle_err; //后验估计 //*************
Q_bias += K_1 * Angle_err; //后验估计 //*************
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度 //*************
return Angle; //*************
} //*************
/**************************************************************************************
角度融合
**************************************************************************************/ //*************
float kalman_filter(float angle_m,float gyro_m)//非矩阵卡尔曼滤波 //*************
{ //*************
sta
ti
c float x=0; //*************
static float P=0.000001; //*************
static float Q=0.000001; //*************
static float R=0.35;//0.35 //*************
static float k=0; //*************
gyro_m=W_K*gyro_m;//角速度修正 //*************
x=x+gyro_m*dt; //*************
P=P+Q; //*************
k=P/(P+R); //*************
x=x+k*(angle_m-x); //*************
P=(1-k)*P; //*************
return x; //*************
} //*************
//*************
float GYRO_Integration; //*************
float Complement_Filter(float angle_m,float gyro_m)//互补融合滤波//************* //*************
{ //*************
float Angle_Filter_Temp;//互补融合滤波中间量 //*************
float Angle_Difference_Value;//融合后的角度与加速度静态角度差值,作为反馈量加入积分回路,让静差为0 //*************
//*************
Angle_Filter_Temp=GYRO_Integration; //*************
gyro_m=W_K*gyro_m;//角速度修正 //*************
Angle_Difference_Value=(angle_m-Angle_Filter_Temp)*1.0;//融合后的角度与加速度静态角度差值 //*************
GYRO_Integration=GYRO_Integration+(gyro_m+Angle_Difference_Value )*dt; //*************
return GYRO_Integration; //*************
} //*************
//*************
/************************上面滤波过程很重要,很重要,很重要,重要的事情说三遍**************************************************************/
/************************上面滤波过程很重要,很重要,很重要,重要的事情说三遍**************************************************************/
三种融合.zip
(2016-1-26 02:01 上传)
1.31 KB, 下载次数: 9
回帖
(3)
bzr1108lzg
2016-1-28 17:17:25
的飒飒大飒飒发放
的飒飒大飒飒发放
举报
fengqu
2016-3-9 12:12:19
很不错。。。。
很不错。。。。
举报
刘挡
2016-3-9 16:46:51
学学 学学 非常好的资料
学学 学学 非常好的资料
举报
更多回帖
rotate(-90deg);
回复
相关帖子
Angle
刷机时
用到
的
三种
工具分别是什么
2021-12-27
1145
永磁同步电机
三种
无速度的方法
2021-08-27
1290
STM32的
三种
启动模式
2021-08-05
998
关于高速ADC的选择与应用 你想要的都
在这里
2021-05-25
2089
常见
三种
接口详解
2019-08-02
2945
请问这个电感
在这里
是滤波吗?
2019-03-05
2323
你要的自供电都
在这里
2016-12-08
3379
学习中遇到问题可以
在这里
发帖请教吗
2014-09-25
2290
为什么
在这里
下的文件
都有
损坏
2012-03-30
2770
为什么
在这里
下的文件
都有
损坏
2012-03-30
4006
发帖
登录/注册
20万+
工程师都在用,
免费
PCB检查工具
无需安装、支持浏览器和手机在线查看、实时共享
查看
点击登录
登录更多精彩功能!
首页
论坛版块
小组
免费开发板试用
ebook
直播
搜索
登录
×
20
完善资料,
赚取积分