q: a six-axis gyroscope of how a relatively stable solutions of Euler angles?

Complementary filtering pose estimation

First attach a piece of code


/**
 * 6DOF 互补滤波姿态估计(via Mahony)
 * @param[in] halfT:状态估计周期的一半
 */
const float Kp = 3.5, Ki = 0.05;
float exInt, eyInt, ezInt;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // roll,pitch,yaw 都为 0 时对应的四元数值。
void IMUupdate(float gx, float gy, float gz, float ax,float ay, float az)
{
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    float q0q0 = q0*q0;
    float q0q1 = q0*q1;
    float q0q2 = q0*q2;
    float q1q1 = q1*q1;
    float q1q3 = q1*q3;
    float q2q2 = q2*q2;
    float q2q3 = q2*q3;
    float q3q3 = q3*q3;

    if(ax*ay*az==0)
        return;

    // 第一步：对加速度数据进行归一化
    norm = sqrt(ax*ax + ay*ay + az*az); 
    ax = ax / norm; 
    ay = ay / norm; 
    az = az / norm; 

    // 第二步：DCM矩阵旋转
    vx = 2*(q1q3 - q0q2); 
    vy = 2*(q0q1 + q2q3); 
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;

    // 第三步：在机体坐标系下做向量叉积得到补偿数据
    ex = ay*vz - az*vy ;
    ey = az*vx - ax*vz ;
    ez = ax*vy - ay*vx ;

    // 第四步：对误差进行PI计算，补偿角速度
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    gx = gx + Kp*ex + exInt;
    gy = gy + Kp*ey + eyInt;
    gz = gz + Kp*ez + ezInt;

    // 第五步：按照四元数微分公式进行四元数更新
    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0/norm;
    q1 = q1/norm;
    q2 = q2/norm;
    q3 = q3/norm;

    roll =  atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;     
    pitch =  asinf(2*q1*q3 - 2*q0*q2)*57.3;                                                          
    yaw  =  -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3; 
}

Here briefly explain, first quaternion is valid regardless of the current can be roughly defined as the quaternion is a method to characterize the attitude of the angular velocity.

First, as measured by the amount of three accelerometers ax, ay, az more accurate, and the solution is calculated by the attitude gyro case three reverse thrust can theoretically how much acceleration axis. ax1, ay1, az1. then there is an error ex, ey, ez between the two vectors. This error is defined as this cross product of the two vectors, respectively. That is $Upper mold by the product sin \ theta$ Since the two are unit vectors, so that $sin\theta$
However, this angle is relatively small, we can approximate it two equivalents $\theta ---------sin\theta$
can then correcting the deviation angle of this new attitude. Thus, we skillfully relatively accurate data accelerometer data to correct the deviation larger data gyroscope. (There is a small knowledge obtained after the error with the error and to update links two proportional integral posture, ω = ω + kp * θacca proportional gain kp to control the convergence rate of the accelerometer 2, integral gain ki Control the convergence rate of the gyro bias)

Gyroscopes and accelerometers complementary finally obtained Euler angles (Mahony Algorithm)

Complementary filtering pose estimation

Guess you like