陀螺仪+加速度计卡尔曼

//float gyro_m：陀螺仪测得的量（角速度）

//float incAngle:加计测得的角度值

#define dt 0.0015//卡尔曼滤波采样频率

#define R_angle 0.69 //测量噪声的协方差（即是测量偏差）

#define Q_angle 0.0001//过程噪声的协方差

#define Q_gyro 0.0003 //过程噪声的协方差过程噪声协方差为一个一行两列矩阵float kalmanUpdate(const float gyro_m,const float incAngle)

{

float K_0;//含有卡尔曼增益的另外一个函数，用于计算最优估计值

float K_1;//含有卡尔曼增益的函数，用于计算最优估计值的偏差

float Y_0;

float Y_1

float Rate;//去除偏差后的角速度

float Pdot[4];//过程协方差矩阵的微分矩阵

float angle_err;//角度偏量

float E;//计算的过程量

static float angle = 0; //下时刻最优估计值角度

static float q_bias = 0; //陀螺仪的偏差

static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//过程协方差矩阵

Rate = gyro_m - q_bias;

//计算过程协方差矩阵的微分矩阵

Pdot[0] = Q_angle - P[0][1] - P[1][0];//？？？？？？

Pdot[1] = - P[1][1];

Pdot[2] = - P[1][1];

Pdot[3] = Q_gyro;//？？？？？？

angle += Rate * 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;

angle_err = incAngle - angle; //计算角度偏差

E = R_angle + P[0][0];

K_0 = P[0][0] / E; //计算卡尔曼增益

K_1 = P[1][0] / E;

Y_0 = P[0][0];

Y_1 = 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;}

加速度模块角度计算：

如果传感器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 ;

static const float Q_angle = 0.001;

static const float Q_gyro = 0.003;

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;

}