卡尔曼滤波和角度测定.docVIP

? 小车下面就是?L3G4200D + ADXL345 两个模块，加速度模块没固定好，板子太小了没地方打孔，有时间将两个模块焊到一个万能板上应该会容易固定一些。 ? ? ? 加速度模块角度计算： ? ??如果传感器 x 轴朝下， y 轴朝前 ? ??那竖直方向弧度计算公式为： angle = atan2(y, z) ? //结果以弧度表示并介于 -pi 到 pi 之间（不包括 -pi） ? ??如果要换算成具体角度： ? ? angle = atan2(y, z) * (180/3.14)? ? ? ? 陀螺仪角度计算： ? ??式中angle(n)为陀螺仪采样到第n次的角度值； ? ??angle(n-1)为陀螺仪第n-1次采样时的角度值； ? ??gyron为陀螺仪的第n次采样得到的瞬时角速率值； ? ??dt为运行一遍所用时间； ? ? ? ? ? ? angle_n += gyro(n) * dt //积分计算 ? ? ? ?卡尔曼滤波? ? ? ?网上找的kalman滤波，具体代码如下?? static?const?float?dt?=?0.02;?static?float?P[2][2]?=?{{?1,?0?},?{?0,?1?}};?float?angle;?float?q_bias;?//偏心、倾向于float?rate;?//比率static?const?float?R_angle?=?0.5?;?//R倾角static?const?float?Q_angle?=?0.001;?//Q倾角static?const?float?Q_gyro??=?0.003;?//Q陀螺仪float?stateUpdate(const?float?gyro_m){?/*状态更新*/float?q;?float?Pdot[4];?q?=?gyro_m?-?q_bias;?Pdot[0]?=?Q_angle?-?P[0][1]?-?P[1][0];????/*?0,0?*/?Pdot[1]?=?-?P[1][1];?????????????/*?0,1?*/?Pdot[2]?=?-?P[1][1];?????????????????/*?1,0?*/?Pdot[3]?=?Q_gyro;?????/*?1,1?*/?rate?=?q;?angle?+=?q?*?dt;?P[0][0]?+=?Pdot[0]?*?dt;?P[0][1]?+=?Pdot[1]?*?dt;?P[1][0]?+=?Pdot[2]?*?dt;?P[1][1]?+=?Pdot[3]?*?dt;?return?angle;?}?float?kalmanUpdate(const?float?incAngle)?{?float?angle_m?=?incAngle;?float?angle_err?=?angle_m?-?angle;?float?h_0?=?1;?const?float?PHt_0?=?h_0*P[0][0];?/*?+?h_1*P[0][1]?=?0*/?const?float?PHt_1?=?h_0*P[1][0];?/*?+?h_1*P[1][1]?=?0*/?float?E?=?R_angle?+(h_0?*?PHt_0);?float?K_0?=?PHt_0?/?E;?float?K_1?=?PHt_1?/?E;?float?Y_0?=?PHt_0;??/*h_0?*?P[0][0]*/?float?Y_1?=?h_0?*?P[0][1];???P[0][0]?-=?K_0?*?Y_0;?P[0][1]?-=?K_0?*?Y_1;?P[1][0]?-=?K_1?*?Y_0;?P[1][1]?-=?K_1?*?Y_1;?angle?+=?K_0?*?angle_err;?q_bias?+=?K_1?*?angle_err;?return?angle;? 复制代码 }?? ? ? ? ? 波形显示? ? ? ? ?测试说明——单片机采集加速度和陀螺仪的信号，并使用上面的kalman滤波，计算出最优倾角，通过串口发送到pc机，pc机运行的串口示波器将相关波形显示出来。? ? ? ? ?1、蓝色为加速度换算后的角度。? ? ? ? ?2、黄色为陀螺仪直接积分后的角度。? ? ? ? ?3、红色为kalman滤波后的角度。? ? ? ? ? ? ? ? 用手指敲小车可以看到加速度模块计算获取的角度震动比较厉害，经过卡尔曼滤波后的波形相对平缓一些。? ? ? ? ? ? ? ? 局部放大看一下曲线还是很优美的哦，哈。。 ? ? ?