陀螺仪传感器建模与卡尔曼姿态解算_陀螺仪角度计算公式

陀螺仪传感器建模与卡尔曼姿态解算
说到姿态解算，目前用到最多的应该是“六轴加速度陀螺仪传感器+卡尔曼滤波算法”。网上的代码和卡尔曼滤波算法的教程很多，但大多数都是分开的两块，很少有将其结合在一起的。
本篇博客重点解释陀螺仪加速度计传感器在姿态解算方面如何建立模型，以及如何编写相应的代码。
一.模型建立
对于角度计算，我们有如下公式：

其中 为陀螺仪测得的角速度
由于现实中陀螺仪存在漂移，所以对上式进行修正如下

其中Gyro为陀螺仪静态漂移
由此我们建立状态方程：

状态量：

测量量：


二.卡尔曼5个公式

三。编程（C语言）







至此，陀螺仪加速度计的建模和编程工作全部完毕，代码摘至网络，本人只是将其与书本中公式结合，希望对读者有所帮助
附：（完整代码）
`//Kalman滤波//
//-------------------------------------------------------
static float Q_angle=0.001, Q_gyro=0.001, R_angle=5, dt=0.004;
//Q增大，动态响应增大
static float Pk[2][2] = { {1, 0}, {0, 1 }};

static float Pdot[4] ={0,0,0,0};

static float q_bias=0, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m)
{
Angle+=(gyro_m-q_bias) * dt; ///预测值
Pdot[0]=Q_angle - Pk[0][1] - Pk[1][0];
Pdot[1]=- Pk[1][1];
Pdot[2]=- Pk[1][1];
Pdot[3]=Q_gyro;

Pk[0][0] += Pdot[0] * dt;
Pk[0][1] += Pdot[1] * dt;
Pk[1][0] += Pdot[2] * dt;
Pk[1][1] += Pdot[3] * dt;

angle_err = angle_m -Angle;///测量值-预测值

PCt_0 =  Pk[0][0];
PCt_1 =  Pk[1][0];

E = R_angle + PCt_0;

K_0 = PCt_0 / E; ///卡尔曼增益
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = Pk[0][1];

Pk[0][0] -= K_0 * t_0;
Pk[0][1] -= K_0 * t_1;
Pk[1][0] -= K_1 * t_0;
Pk[1][1] -= K_1 * t_1;

Angle+= K_0 * angle_err; ///最优角度=预测值+卡尔曼增益*(测量值-预测值)
q_bias	+= K_1 * angle_err;
Angle_Speed = gyro_m-q_bias;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

}`