[C] 纯文本查看 复制代码
#include #include #include #include typedef unsigned char uchar;typedef unsigned short ushort;typedef unsigned int uint;//******角度参数************float Gyro_y; //Y轴陀螺仪数据暂存float Angle_gy; //由角速度计算的倾斜角度float Accel_x; //X轴加速度值暂存，问为有X轴或者是说Y轴。float Angle_ax; //由加速度计算的倾斜角度float Angle; //小车最终倾斜角度uchar value;//******卡尔曼参数************// Q_angle、Q_gyro、R_angle、C_0、Angle_err、PCt_0, PCt_1, E//都是些什么东西float code Q_angle=0.001; float code Q_gyro=0.003;float code R_angle=0.5;float code dt=0.01; //dt为kalman滤波器采样时间;char code C_0 = 1;float xdata Q_bias, Angle_err; //Q_bias是漂移float xdata PCt_0, PCt_1, E;float xdata K_0, K_1, t_0, t_1;float xdata Pdot[4] ={0,0,0,0};float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };//*********************************************************// 卡尔曼滤波//*********************************************************//Kalman滤波，20MHz的处理时间约0.77ms；void Kalman_Filter(float Accel,float Gyro)//这两个参数是什么{Angle+=(Gyro - Q_bias) * dt; //先验估计Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分//协方差的意思是：cov（x，y）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; //输出值(后验估计)的微分=角速度}[C] 纯文本查看 复制代码
[C] 纯文本查看 复制代码
[C] 纯文本查看 复制代码
[C] 纯文本查看 复制代码
[C] 纯文本查看 复制代码
[C] 纯文本查看 复制代码
//*********************************************************// 倾角计算（卡尔曼融合）//*********************************************************void Angle_Calcu(void) {//------加速度--------------------------//范围为2g时，换算关系：16384 LSB/g//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14//因为x>=sinx,故乘以1.3适当放大Accel_x = GetData(ACCEL_XOUT_H); //读取X轴加速度Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度）Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度, //-------角速度-------------------------//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)Gyro_y = GetData(GYRO_YOUT_H); //静止时角速度Y轴输出为-30左右Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值,负号为方向处理 //Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.//-------卡尔曼滤波融合-----------------------Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角/*//-------互补滤波-----------------------//补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与 //陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度//0.5为放大倍数，可调节补偿度；0.01为系统周期10msAngle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/ }
2019-3-28 15:56:50
请问：你的程序哪来的？ 能不能共享一下 我最近在研究卡尔曼滤波 想搜集这方面的程序 尤其是能再单片机上运行的
请问：你的程序哪来的？ 能不能共享一下 我最近在研究卡尔曼滤波 想搜集这方面的程序 尤其是能再单片机上运行的
举报
2019-3-28 16:04:10
如果可以的话 加一下QQ 136 981 3157 备注：kalman
如果可以的话 加一下QQ 136 981 3157 备注：kalman
举报
