推荐星级：
- 1
- 2
- 3
- 4
- 5
陀螺和加速度核心程序详解
资料介绍
// 叉积法融合陀螺和加速度。
void mix_gyrAcc_crossMethod(quaternion_yuandian * attitude,const float gyr[3],const float acc[3],float interval)
{
const static float FACTOR = 0.001;//两个重力矢量叉积后所乘的系数 p，用于和陀螺仪积分角度相叠加来修正陀螺仪（这里只用了比例 p，没用积分 i， ）
//FACTOR 为 1，则完全信任加速度计，为 0，则完全信任陀螺仪
float w_q = attitude->w;//w=cos(alpha/2)
float x_q = attitude->x;//x=ax*sin(alpha/2)
float y_q = attitude->y;//y=ay*sin(alpha/2)
float z_q = attitude->z;//z=az*sin(alpha/2)
float x_q_2 = x_q * 2;
float y_q_2 = y_q * 2;
float z_q_2 = z_q * 2;
//
// 加速度计的读数，单位化。
float a_rsqrt = math_rsqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
float x_aa = acc[0] * a_rsqrt;
float y_aa = acc[1] * a_rsqrt;
float z_aa = acc[2] * a_rsqrt;
部分文件列表
| 文件名 | 文件大小 | 修改时间 |
| 陀螺和加速度核心程序详解.pdf | 63KB | 2018-07-31 22:43:42 |
全部评论(1)
2023-02-14 15:19:24wzszzxj
厉害，学习了