论坛首页 › 飞行器 › 请教一下移植过PIX代码的大神关于里面EKF的相关问题。

makeflyeasy 发表于 2016-4-10 22:42:47

请教一下移植过PIX代码的大神关于里面EKF的相关问题。

本帖最后由 makeflyeasy 于 2016-4-11 09:58 编辑

小弟以前用的别的姿态测算程序，最近想试试PIX的EKF程序，也就是EKF_att_pos，但是移植完毕后发现输出的姿态和加速度无关，也就是加速度根本就不参与姿态融合计算，出来的姿态纯粹是用陀螺仪积分积出来的，通读了整个PIX的姿态融合代码，都找不到EKF里面加速度值在哪里参与了姿态融合。
小弟先讲下我对PIX代码EKF部分的理解吧，麻烦众位大神看看对不对。

首先是EKF参数初始化，包括各种噪声的预设值。

这个图中的值我已经按照代码里面的推荐值初始化了。


之后是如果EKF没有初始化，则初始化它。这个也已经做了。

其次就是EKF主程序了，流程是填数据(polldata)运行EKF主程序，主程序又包括惯导方程更新，也就是六轴数据更新，这个是必须的，然后这些是可选的:GPS融合，磁力计融合，光流融合，气压计融合，测距仪融合。
之后就是发布各种数据。 整个EKF就完了，EKF和发布数据中间有个地形更新不知道是干啥用的。


我是这么移植的，这是我的进程循环过程:

首先是获取九轴数据
其次是填充数据：

再者就运行惯导方程更新，但是问题来了，加速度计的值，这里没有直接用到，用到的是Acc*dt，也就是dAngIMU；
但是推演下去，发现加速度的值在EKF主程序里面根本就没有融合到最终姿态里面去，也就是没有用加速度计的值修正最终姿态。

可以看到dVelIMURel = dVelIMU;,这个唯一和加速度有关的参数,在四元数归一化并转化为最终四元数输出时，都没有参与姿态融合运算。


之后dVelIMURel 参与了delVelNav参数的运算，但是这个参数看后面可以知道是加速度积分算速度用的，根本就没有参与姿态融合。然后这个程序就完了，EKF主程序部分又是对GPS等数据融合的部分，又没有加速度直接参与。


这样就导致了我的结果，那就是姿态数据虽然出来了，但是是纯陀螺仪积分积出来的，加速度计没有参与姿态修正。不过我将代码下到PIX的硬件里面姿态又是正常的，我移植后又是不正常的。

我的EKF周期是0.002s，dtIMU什么的我都改过，确认没有问题，想请教下各位大神，PIX的EKF里面加速度到底是怎么参与姿态融合运算的？是不是我哪里配置错了导致加速度没有参与姿态运算？

以下是我的程序截取:
这是EKF参数初始化
dAngBiasSigma = 1e-07f;//_parameters.gbias_pnoise;
      dVelBiasSigma = 1e-05f;//_parameters.abias_pnoise;
      magEarthSigma = 0.0003f;//_parameters.mage_pnoise;
      magBodySigma= 0.0003f;//_parameters.magb_pnoise;
//gndHgtSigma= 0.02f;
      vneSigma = 0.3f;//_parameters.velne_noise;
      vdSigma = 0.3f;//_parameters.veld_noise;
      posNeSigma = 0.5f;//_parameters.posne_noise;
      posDSigma = 1.25f;//_parameters.posd_noise;
      magMeasurementSigma = 0.05f;//_parameters.mag_noise;
      gyroProcessNoise = 0.015f;//_parameters.gyro_pnoise;
      accelProcessNoise = 0.125f;//_parameters.acc_pnoise;
      airspeedMeasurementSigma = 1.4f;//_parameters.eas_noise;
      rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF这是数据输入:polldata有乱码，请无视这里陀螺仪数据的单位是弧度/s   加速度计数据单位是g
dAngIMU.x = 0.5f * (angRate.x + X_Gyro*0.0174533f) * dtIMU;
      dAngIMU.y = 0.5f * (angRate.y + Y_Gyro*0.0174533f) * dtIMU;
      dAngIMU.z = 0.5f * (angRate.z + Z_Gyro*0.0174533f) * dtIMU;
      
      angRate.x=X_Gyro*0.0174533f; //ÊäÈë½ÇËÙ¶È,µ¥Î»:rad/s
      angRate.y=Y_Gyro*0.0174533f; //0.174533ΪPI/180 Ä¿µÄÊǽ«½Ç¶Èת»¡¶È
      angRate.z=Z_Gyro*0.0174533f;
      
      dVelIMU.x = 0.5f * (accel.x + X_Accel) * dtIMU;
      dVelIMU.y = 0.5f * (accel.y + Y_Accel) * dtIMU;
      dVelIMU.z = 0.5f * (accel.z + Z_Accel) * dtIMU;
      
accel.x=X_Accel; //ÊäÈë¼ÓËÙ¶È,µ¥Î»:g
      accel.y=Y_Accel;
      accel.z=Z_Accel;
      
      magData.x=MAG_X;//ÊäÈë´ÅÁ¦¼ÆÊý¾Ý
      magData.y=MAG_Y;
      magData.z=MAG_Z;这是更新惯导方程的，和PIX的代码是一样的了，我觉得加速度是在这里面参与姿态融合的，但是找不到在哪里。
void UpdateStrapdownEquationsNED(void)
{
    Vector3f delVelNav;
          static float accNavMagHorizontal;
    float q00;
    float q11;
    float q22;
    float q33;
    float q01;
    float q02;
    float q03;
    float q12;
    float q13;
    float q23;
    float rotationMag;
    float qUpdated;
    float quatMag;
    float deltaQuat;
    const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS);

          if(First_Init_EKF==true)
                {
                        float initVelNED = {0.0f, 0.0f, 0.0f};
            InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
            posNE = 0.0f;
            posNE = 0.0f;
                }
    // 删除传感器偏置错误
    correctedDelAng.x = dAngIMU.x - states;
    correctedDelAng.y = dAngIMU.y - states;
    correctedDelAng.z = dAngIMU.z - states;

    Vector3f dVelIMURel;

    dVelIMURel.x = dVelIMU.x;
    dVelIMURel.y = dVelIMU.y;
    dVelIMURel.z = dVelIMU.z - states;

    delAngTotal.x += correctedDelAng.x;
    delAngTotal.y += correctedDelAng.y;
    delAngTotal.z += correctedDelAng.z;

    // Apply corrections for earths rotation rate and coning errors
    // * and + operators have been overloaded
    correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
    prevDelAng = correctedDelAng;

    // 转化为等效四元数旋转矢量
    rotationMag = correctedDelAng.length();
    if (rotationMag < 1e-12f)
    {
      deltaQuat = 1.0;
      deltaQuat = 0.0;
      deltaQuat = 0.0;
      deltaQuat = 0.0;
    }
    else
    {
      // We are using double here as we are unsure how small
      // the angle differences are and if we get into numeric
      // issues with float. The runtime impact is not measurable
      // for these quantities.
      deltaQuat = cos(0.5*(double)rotationMag);
      float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
      deltaQuat = correctedDelAng.x*rotScaler;
      deltaQuat = correctedDelAng.y*rotScaler;
      deltaQuat = correctedDelAng.z*rotScaler;
    }

    // Update the quaternions by rotating from the previous attitude through
    // the delta angle rotation quaternion
    qUpdated = states*deltaQuat - states*deltaQuat - states*deltaQuat - states*deltaQuat;
    qUpdated = states*deltaQuat + states*deltaQuat + states*deltaQuat - states*deltaQuat;
    qUpdated = states*deltaQuat + states*deltaQuat + states*deltaQuat - states*deltaQuat;
    qUpdated = states*deltaQuat + states*deltaQuat + states*deltaQuat - states*deltaQuat;

    // Normalise the quaternions and update the quaternion states
    quatMag = sqrtf(sq(qUpdated) + sq(qUpdated) + sq(qUpdated) + sq(qUpdated));
    if (quatMag > 1e-16f)
    {
      float quatMagInv = 1.0f/quatMag;
      states = quatMagInv*qUpdated;
      states = quatMagInv*qUpdated;
      states = quatMagInv*qUpdated;
      states = quatMagInv*qUpdated;
    }
    // Calculate the body to nav cosine matrix
    q00 = sq(states);
    q11 = sq(states);
    q22 = sq(states);
    q33 = sq(states);
    q01 =states*states;
    q02 =states*states;
    q03 =states*states;
    q12 =states*states;
    q13 =states*states;
    q23 =states*states;

    Tbn.x.x = q00 + q11 - q22 - q33;
    Tbn.y.y = q00 - q11 + q22 - q33;
    Tbn.z.z = q00 - q11 - q22 + q33;
    Tbn.x.y = 2*(q12 - q03);
    Tbn.x.z = 2*(q13 + q02);
    Tbn.y.x = 2*(q12 + q03);
    Tbn.y.z = 2*(q23 - q01);
    Tbn.z.x = 2*(q13 - q02);
    Tbn.z.y = 2*(q23 + q01);

    Tnb = Tbn.transpose();

    // transform body delta velocities to delta velocities in the nav frame
    // * and + operators have been overloaded
    //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
    delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
    delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
    delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;

    // calculate the magnitude of the nav acceleration (required for GPS
    // variance estimation)
    accNavMag = delVelNav.length()/dtIMU;

    //First order low-pass filtered magnitude of horizontal nav acceleration
    Vector3f derivativeNav = (delVelNav / dtIMU);
    float derivativeVelNavMagnitude = sqrtf(sq(derivativeNav.x) + sq(derivativeNav.y));
    accNavMagHorizontal = accNavMagHorizontal * 0.95f + derivativeVelNavMagnitude * 0.05f;

    // If calculating position save previous velocity
    float lastVelocity;
    lastVelocity = states;
    lastVelocity = states;
    lastVelocity = states;

    // Sum delta velocities to get velocity
    states = states + delVelNav.x;
    states = states + delVelNav.y;
    states = states + delVelNav.z;

    // If calculating postions, do a trapezoidal integration for position
    states = states + 0.5f*(states + lastVelocity)*dtIMU;
    states = states + 0.5f*(states + lastVelocity)*dtIMU;
    states = states + 0.5f*(states + lastVelocity)*dtIMU;

    // Constrain states (to protect against filter divergence)
    ConstrainStates();

    // update filtered IMU time step length
    dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
}

makeflyeasy 发表于 2016-4-10 23:08:51

啊，有木有人看得懂啊，我把PIX的EKF代码通读好几遍了，就是不知道加速度计怎么参与姿态融合了，不像以前的互补滤波程序，一眼就看出了加速度计是怎么融合进去的了。

makeflyeasy 发表于 2016-4-11 10:26:34

只有自己顶喽

makeflyeasy 发表于 2016-4-12 10:24:31

哎，看来是没人懂这玩意的原理了，用EKF2算了。

makeflyeasy 发表于 2016-4-14 08:55:42

再顶顶，真的就没有人研究过PIX的扩展卡尔曼算法吗？

小李非刀 发表于 2016-4-14 14:03:17

没研究过呢，帮顶，等高人。。。。

熵之矢 发表于 2016-4-26 10:38:47

你看的是pixhawk的stack还是APM的？

笑笑我笑了 发表于 2016-4-26 11:01:29

我看的pixhawk中的姿态估计有个.m文件，很容易就搞懂了啊，没LZ那么复杂。

查看完整版本: 请教一下移植过PIX代码的大神关于里面EKF的相关问题。