四轴程序理解

四元数更新算法

1. /****************************************************/  

1.      delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);      
2.     q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT;            // 整合四元数率     四元数微分方程    四元数更新算法，二阶毕卡法  
3.     q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;  
4.     q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;  
5.     q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT;         
6. /****************************************************/  

1. /* 
2.     // integrate quaternion rate and normalise，四元数更新算法，一阶龙格-库塔法 
3.     q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; 
4.     q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; 
5.     q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; 
6.     q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;   
7. */  

卡尔曼滤波算法（拷贝）

卡尔曼滤波：

1. //============================================================================ 
   
2. //                 卡尔曼滤波参数
   
3. float Q_angle=0.001,
   
4.       Q_gyro=0.005,
   
5.       R_angle=0.5,
   
6.       dt=0.0055,//注意：dt的取值为kalman滤波器采样时间;
   
7.       angle,
   
8.       angle_dot;    
   
9. 
   
10. float P[2][2] = {            { 1, 0 },
    
11.                              { 0, 1 }
    
12.                  };                  //alpha_k
    
13.     
    
14. float Pdot[4] ={0,0,0,0};        //Pdot = (u_k-bias)
    
15. const char C_0 = 1;       //H = [1 0]矩阵，用于提取
    
16. float   q_bias ,           //陀螺仪常值偏差bias 
    
17.         angle_err,        //观测值减去估计值
    
18.         PCt_0, PCt_1,     //计算K的中间步骤，分子 PCT = PH_T
    
19.         E,                //计算K的中间步骤，分母 E =(HPH_T + R)
    
20.         K_0, K_1,         //K为卡尔曼增益,K = P(K),矩阵
    
21.         t_0, t_1;         //更新alpha的中间步骤  
    
22. 
    
23. 
    
24. 
    
25. 
    
26. //============================================================================
    
27. //函数名称：Kalman_Filter
    
28. //功能概要：卡尔曼滤波
    
29. 
    
30. void Kalman_Filter(float angle_m,float gyro_m)          //gyro_m:gyro_measure
    
31. {
    
32. 
    
33.   
    
34. 
    
35. 
    
36. //        卡尔曼滤波开始
    
37.     
    
38.     angle+=(gyro_m-q_bias) * dt;//先验估计
    
39. 
    
40. 
    
41.     Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
    
42.     Pdot[1]=- P[1][1];
    
43.     Pdot[2]=- P[1][1];
    
44.     Pdot[3]=Q_gyro;                      //Pdot = (u_k-bias)
    
45.     
    
46.     P[0][0] += Pdot[0] * dt;        // Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
    
47.     P[0][1] += Pdot[1] * dt;
    
48.     P[1][0] += Pdot[2] * dt;
    
49.     P[1][1] += Pdot[3] * dt;        //alpha_k = alpha_k-1 + [(u_k-bias)*dt]
    
50.     
    
51.     
    
52.     angle_err = angle_m - angle;    //zk-先验估计 观测值减去估计值
    
53.     
    
54.     
    
55.     PCt_0 = C_0 * P[0][0];
    
56.     PCt_1 = C_0 * P[1][0];          //计算K的中间步骤，分子 PCT = PH^T
    
57.     
    
58.     E = R_angle + C_0 * PCt_0;      //计算K的中间步骤，分母 E =(HPH^T + R)
    
59.     
    
60.     K_0 = PCt_0 / E;        //K =   PH^T / (HPH^T + R)
    
61.     K_1 = PCt_1 / E;
    
62.     
    
63.     t_0 = PCt_0;            //更新alpha的中间步骤
    
64.     t_1 = C_0 * P[0][1];
    
65. 
    
66.     P[0][0] -= K_0 * t_0;   //后验估计误差协方差
    
67.     P[0][1] -= K_0 * t_1;
    
68.     P[1][0] -= K_1 * t_0;   //alpha_k+1 = alpha_k - KH*alpha_k
    
69.     P[1][1] -= K_1 * t_1;
    
70.     
    
71.     
    
72.     angle+= K_0 * angle_err;//后验估计   更新估计值作为最终结果
    
73.     q_bias+= K_1 * angle_err;//后验估计   更新bias的值
    
74.     angle_dot=gyro_m-q_bias;//输出值（后验估计）的微分 = 角速度
    
75.     
    
76. 
    
77. //                 卡尔曼滤波结束
    
78. 
    
79. 
    
80. 
    
81. }

来源http://forum.eepw.com.cn/thread/264504/1

IMU加速度积分得到速度参考 ：  http://blog.csdn.net/me_sky_2012/article/details/31350909

数字信号处理：IIR滤波

单变量卡尔曼滤波：http://www.amobbs.com/thread-5475821-2-1.html