这里是一个国外的朋友的Kalman滤波程序,我也是利用这个程序:
这是使用卡尔曼滤波器来完成对陀螺仪和加速度计滤波处理!非常实用和有效!
我自己简单分析一下,有错误之处请大家指出。
// --- Kalman filter module----------------------------------------------------------------------------
float Q_angle=0.001; //0.001 这个是加速度计得到的角度误差(方差)
float Q_gyro =0.003;//0.003 这个是陀螺仪得到的角度误差(方差)
float R_angle=0.03;//0.03 这个是激励误差(方差)
float x_angle = 0; 这个是X的角度值,也就是滤波后得到的角度值
float x_bias = 0; 这个是白噪声
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; 这些是卡拉曼递推运算的保存值
float dt, y, S;
float K_0, K_1; 这些是卡拉曼递推运算的保存值
newAngle是加速度计得到的角度,newRate是陀螺仪测得的角速度,looptime是程序循环的时间
float kalmanCalculate(float newAngle, float newRate,int looptime)
{
dt = float(looptime)/1000; 通过对循环时间的计算得到陀螺仪的采用时间
x_angle += dt * (newRate - x_bias); 计算陀螺仪的角度
P_00 +=- dt * (P_10 + P_01) + Q_angle * dt; 以下4行是对递推过程值的计算
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; 以下2行是对递推过程值的计算
K_1 = P_10 / S;
x_angle +=K_0 * y; 用Y的值修正陀螺仪的角度值,得到最终的滤波值
x_bias+=K_1 * y; 计算新的白噪声
P_00 -= K_0 * P_00; 以下4行是对递推过程值的计算
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return x_angle; 返回滤波后的角度
} 牛人 版主的精神可嘉 值得学习继续关注 很久没有更新了,今天来聊聊这段时间累积的经验。
之前使用了Gy401的传感器,和九鹰3轴陀螺仪系统中的陀螺仪。先说说Gy401的传感器,这是一个很高级的传感器,模拟量输出,为了得到数字数据使用了一片带有12位有效数的ADC的MCU首先进行ADC转换并且对转换后的数据进行滤波处理,使用了kalman滤波器,滤波效果非常显著。
再说说九鹰的陀螺仪,3轴陀螺仪由一个单轴Z陀螺仪和一个XY 2轴陀螺仪,需要对3个陀螺仪进行ADC转换,这样就需要非常快的转换速度,否则再转换时间上可能会有损失,条件有限没有使用专用的多通道ADC采样IC,这类IC可以在ADI的官网上找到,性能非常优秀!比MCU集成的ADC好的多,而且基本都是高速ADC采样,还有些是多通道同时采样!
在得到滤波后的数据就可以用积分的方式得到旋转的角度。使用的计算方法为:
Angle = AdcValue * dt
Angle就是计算后的角度,AdcValue是陀螺仪采样滤波后的数据,而dt则是系统每次采样的时间。
得到了角度接下来就是将角度数据作为误差数据输入到PID算法,计算出执行机构所需要的数据了。
为了得到较好的性能,需要选择运算速度更快的MCU,而其绝大多数运算都是使用浮点数,这就进一步提高了对MCU的负担。
页:
1
[2]