陀螺仪传感器建模与卡尔曼姿态解算

陀螺仪传感器建模和卡尔曼姿态解决方案
说到姿态解决，最常用的应该是六轴加速陀螺仪传感器 卡尔曼滤波算法。网上代码和卡尔曼滤波算法的教程很多，但大部分都是分开的，很少结合在一起。
本博客重点介绍陀螺仪加速度计传感器如何建立姿态解算模型，以及如何编写相应的代码。
一.模型建立
我们对角度计算有以下公式：

其中 陀螺仪测量的角速度
由于陀螺仪在现实中存在漂移，上述类型的修正如下

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

状态量：

测量量：


二.五个卡尔曼公式

三。编程（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，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; 

}`