Heading results drift ovr time in EKF

Other Hardware Robotics
1

I recently build a multi-rate EKF for attitude estimation for my drone, I use a bmm150 magnetometer and bmi088 imu, and no matter how much I tune my filter my heading drift over time, roll, and pitch are precise, I put compass RMS noise in measurement noise but still not work, one question can we put RMS noise directly put on the R matrix? what should I do so my heading not drift? not that heading is so important for me as I build a GPS drone.

#include <Arduino.h>
#include <ArduinoEigenDense.h>

using namespace Eigen;


class EKF
{
private:
    /* data */
public:
    EKF(/* args */);

    float mMagDataAvailable;
    float mAccDataAvailable;

    Vector4f xhat = Vector4f::Zero();
    Vector4f xhatPrev = Vector4f::Zero();
    Vector4f xhatNew = Vector4f::Zero();

    VectorXf mErrorMag = VectorXf::Zero(3);
    VectorXf mErrorAcc = VectorXf::Zero(3);

    VectorXf mAccPredicted = VectorXf::Zero(3);
    VectorXf mMagPredicted = VectorXf::Zero(3);

    VectorXf mAccRef = VectorXf::Zero(3);
    VectorXf mMagRef = VectorXf::Zero(3);

    Vector3f eularAngle = Vector3f::Zero();

    MatrixXf pHat = MatrixXf::Identity(4, 4);
    MatrixXf pHatPrev = MatrixXf::Identity(4, 4);
    MatrixXf pHatNew = MatrixXf::Identity(4, 4);


    MatrixXf Q = MatrixXf::Zero(4, 4);
    MatrixXf Wt = MatrixXf::Zero(4, 3);
    MatrixXf gyro_noise = MatrixXf::Identity(3, 3);

    MatrixXf F_t = MatrixXf::Zero(4, 4);

    MatrixXf Ka = MatrixXf::Zero(4, 3);
    MatrixXf Km = MatrixXf::Zero(4, 3);

    MatrixXf Ra = MatrixXf::Identity(3, 3);
    MatrixXf Rm = MatrixXf::Identity(3, 3);

    MatrixXf Sa = MatrixXf::Zero(3, 3);
    MatrixXf Sm = MatrixXf::Zero(3, 3);


    MatrixXf Ha = MatrixXf::Zero(3, 4);
    MatrixXf Hm = MatrixXf::Zero(3, 4);

    MatrixXf I = MatrixXf::Identity(4, 4);
    MatrixXf rotMatEuler = MatrixXf::Zero(3, 3);

    void initialize(float ax, float ay, Vector3f mMag);
    void fuseAngularValocity(float dt, Vector3f mGyro);
    void fuseAcceleration(Vector3f mAcc);
    void fuseCompass(Vector3f mMag);
    Matrix3f q2R(Vector4f q);

    void calculateJacobianOfAccel(VectorXf xhat, VectorXf ref);
    void calculateJacobianOfCompass(VectorXf xhat, VectorXf ref);
    void getRotationMatrix(VectorXf q);
    void getEularAngles(VectorXf q);

    float rmse;
    int i = 0;
    float error;
    float meanSquaredErrors, sumSquaredErrors;
};


#include <EKF.h>

EKF::EKF(/* args */)
{
}

void EKF::fuseAngularValocity(float dt, Vector3f mGyro)
{

    float qw = xhatPrev(0);
    float qx = xhatPrev(1);
    float qy = xhatPrev(2);
    float qz = xhatPrev(3);

    float wx = mGyro(0);
    float wy = mGyro(1);
    float wz = mGyro(2);

    float DT = 0.5 * dt;

    xhat(0) = qw - DT * wx * qx - DT * wy * qy - DT * wz * qz;
    xhat(1) = qx + DT * wx * qw - DT * wy * qz + DT * wz * qy;
    xhat(2) = qy + DT * wx * qz + DT * wy * qw - DT * wz * qx;
    xhat(3) = qz - DT * wx * qy + DT * wy * qx + DT * wz * qw;

    F_t(0, 0) = 1.0f;
    F_t(0, 1) = -DT * wx;
    F_t(0, 2) = -DT * wy;
    F_t(0, 3) = -DT * wz;

    F_t(1, 0) = DT * wx;
    F_t(1, 1) = 1.0f;
    F_t(1, 2) = DT * wz;
    F_t(1, 3) = -DT * wy;

    F_t(2, 0) = DT * wy;
    F_t(2, 1) = -DT * wz;
    F_t(2, 2) = 1.0f;
    F_t(2, 3) = DT * wx;

    F_t(3, 0) = DT * wz;
    F_t(3, 1) = DT * wy;
    F_t(3, 2) = -DT * wx;
    F_t(3, 3) = 1.0f;

    Wt(0, 0) = -qx * DT;
    Wt(0, 1) = -qy * DT;
    Wt(0, 2) = -qz * DT;

    Wt(1, 0) = qw * DT;
    Wt(1, 1) = -qz * DT;
    Wt(1, 2) = qy * DT;

    Wt(2, 0) = qz * DT;
    Wt(2, 1) = qw * DT;
    Wt(2, 2) = -qx * DT;

    Wt(3, 0) = -qy * DT;
    Wt(3, 1) = qx * DT;
    Wt(3, 2) = qw * DT;

    Q = Wt * gyro_noise * Wt.transpose();
    pHat = F_t * pHatPrev * F_t.transpose() + Q;
}

void EKF::fuseAcceleration(Vector3f mAcc)
{
    if (mAccDataAvailable)
    {
        mAcc.normalize();

        mAccPredicted = q2R(xhat).transpose() * mAccRef;
        mErrorAcc = mAcc - mAccPredicted;
        calculateJacobianOfAccel(xhat, mAccRef);

        Sa = Ha * pHat * Ha.transpose() + Ra;
        Ka = pHat * Ha.transpose() * Sa.inverse();

        xhatNew = xhat + Ka * mErrorAcc;
        pHatNew = (I - Ka * Ha) * pHat;

        xhatNew.normalize();

        getEularAngles(xhatNew);

        xhatPrev = xhatNew;
        pHatPrev = pHatNew;

        mAccDataAvailable = false;
    }
}

void EKF::fuseCompass(Vector3f mMag)
{

    if (mMagDataAvailable)
    {
        mMag.normalize();
        mMagPredicted = q2R(xhat).transpose() * mMagRef;

        mErrorMag = mMag - mMagPredicted;

        calculateJacobianOfCompass(xhat, mMagRef);

        Sm = Hm * pHat * Hm.transpose() + Rm;
        Km = pHat * Hm.transpose() * Sm.inverse();

        xhatNew = xhat + Km * mErrorMag;
        pHatNew = (I - Km * Hm) * pHat;

        xhatNew.normalize();

        xhatPrev = xhatNew;
        pHatPrev = pHatNew;

        mMagDataAvailable = false;
    }
}

Matrix3f EKF::q2R(Vector4f q)
{
    Matrix3f rotMat;
    rotMat << 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]),
        2.0 * (q[1] * q[2] - q[0] * q[3]),
        2.0 * (q[1] * q[3] + q[0] * q[2]),
        2.0 * (q[1] * q[2] + q[0] * q[3]),
        1.0 - 2.0 * (q[1] * q[1] + q[3] * q[3]),
        2.0 * (q[2] * q[3] - q[0] * q[1]),
        2.0 * (q[1] * q[3] - q[0] * q[2]),
        2.0 * (q[0] * q[1] + q[2] * q[3]),
        1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
    return rotMat;
}

void EKF::calculateJacobianOfAccel(VectorXf xhat, VectorXf ref)
{
    float qw = xhat(0);
    float qx = xhat(1);
    float qy = xhat(2);
    float qz = xhat(3);

    float gx = ref(0);
    float gy = ref(1);
    float gz = ref(2);

    Ha(0, 0) = gy * qz - gz * qy;
    Ha(0, 1) = gy * qy + gz * qz;
    Ha(0, 2) = -2.0f * gx * qy + gy * qx - gz * qw;
    Ha(0, 3) = -2.0f * gx * qz + gy * qw + gz * qx;

    Ha(1, 0) = -gx * qz + gz * qx;
    Ha(1, 1) = gx * qy - 2.0f * gy * qx + gz * qw;
    Ha(1, 2) = gx * qx + gz * qz;
    Ha(1, 3) = -gx * qw - 2.0f * gy * qz + gz * qy;

    Ha(2, 0) = gx * qy - gy * qx;
    Ha(2, 1) = gx * qz - gy * qw - 2.0f * gz * qx;
    Ha(2, 2) = gx * qw + gy * qz - 2.0f * gz * qy;
    Ha(2, 3) = gx * qx + gy * qy;

    Ha *= 2.0f;
}

void EKF::calculateJacobianOfCompass(VectorXf xhat, VectorXf ref)
{
    float qw = xhat(0);
    float qx = xhat(1);
    float qy = xhat(2);
    float qz = xhat(3);

    float rx = ref(0);
    float ry = ref(1);
    float rz = ref(2);

    Hm(0, 0) = ry * qz - rz * qy;
    Hm(0, 1) = ry * qy + rz * qz;
    Hm(0, 2) = -2.0f * rx * qy + ry * qx - rz * qw;
    Hm(0, 3) = -2.0f * rx * qz + ry * qw + rz * qx;

    Hm(1, 0) = -rx * qz + rz * qx;
    Hm(1, 1) = rx * qy - 2.0f * ry * qx + rz * qw;
    Hm(1, 2) = rx * qx + rz * qz;
    Hm(1, 3) = -rx * qw - 2.0f * ry * qz + rz * qy;

    Hm(2, 0) = rx * qy - ry * qx;
    Hm(2, 1) = rx * qz - ry * qw - 2.0f * rz * qx;
    Hm(2, 2) = rx * qw + ry * qz - 2.0f * rz * qy;
    Hm(2, 3) = rx * qx + ry * qy;
    Hm *= 2.0f;
}

void EKF::initialize(float ax, float ay, Vector3f mMag)
{
    mMagRef << 0.80457377, 0.0084067, 0.59379321;
    mAccRef << 0.0f, 0.0f, 1.0f;

    // Optimal Values
    //  gyro_noise(0, 0) = (5.08244208f * 5.08244208f) / 3.5;
    //  gyro_noise(1, 1) = (9.12140635f * 9.12140635f) / 7.5;
    //  gyro_noise(2, 2) = (5.81727448f * 5.81727448f) / 3.5;

    // Ra(0, 0) = 1.56341423f * 1.56341423f;
    // Ra(1, 1) = 1.54743626f * 1.54743626f;
    // Ra(2, 2) = 1.63774555f * 1.63774555f;

    // Rm(0, 0) = 0.50485281f * 0.50485281f;
    // Rm(1, 1) = 0.59562616f * 0.59562616f;
    // Rm(2, 2) = 0.62478246f * 0.62478246f;

    float theta = asinf(ax / 9.807);
    float phi = asinf(ay / (9.807 * cos(theta)));
    // magnetic heading correction due to roll and pitch angle
    float Bxc = mMag(0) * cos(theta) + (mMag(1) * sin(phi) + mMag(2) * cos(phi)) * sin(theta);
    float Byc = mMag(1) * cos(phi) - mMag(2) * sin(phi);

    float psi = atan2(Byc, Bxc);

    float cy = cos(psi * 0.5);
    float sy = sin(psi * 0.5);
    float cp = cos(theta * 0.5);
    float sp = sin(theta * 0.5);
    float cr = cos(phi * 0.5);
    float sr = sin(phi * 0.5);

    xhatPrev(0) = cr * cp * cy + sr * sp * sy;
    xhatPrev(1) = sr * cp * cy - cr * sp * sy;
    xhatPrev(2) = cr * sp * cy + sr * cp * sy;
    xhatPrev(3) = cr * cp * sy - sr * sp * cy;

    // gyro_noise(0, 0) = 5.08244208f * 5.08244208f;
    // gyro_noise(1, 1) = 9.12140635f * 9.12140635f;
    // gyro_noise(2, 2) = 5.81727448f * 5.81727448f;

    gyro_noise(0, 0) = (5.08244208f * 5.08244208f) / 3.5f;
    gyro_noise(1, 1) = (9.12140635f * 9.12140635f) / 7.5f;
    gyro_noise(2, 2) = (2.81727448f * 2.81727448f) / 3.5f ;

    Ra(0, 0) = 1.56341423f * 1.56341423f;
    Ra(1, 1) = 1.54743626f * 1.54743626f;
    Ra(2, 2) = 1.63774555f * 1.63774555f;

    Rm(1, 1) = 1.000562616f * 1.000562616f;
    Rm(0, 0) = 1.000485281f * 1.000485281f;
    Rm(2, 2) = 1.400478246f * 1.400478246f;
}

void EKF::getRotationMatrix(VectorXf q)
{
    // Matrix3f rotMat = Matrix3f::Zero();
    rotMatEuler << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3),
        2 * (q(1) * q(2) - q(0) * q(3)),
        2 * (q(1) * q(3) + q(0) * q(2)),
        2 * (q(1) * q(2) + q(0) * q(3)),
        q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3),
        2 * (q(2) * q(3) - q(0) * q(1)),
        2 * (q(1) * q(3) - q(0) * q(2)),
        2 * (q(2) * q(3) + q(0) * q(1)),
        q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3);
}

void EKF::getEularAngles(VectorXf q)
{
    // float magnitude = q.norm();
    // float epsilon = 1e-6;
    // bool isUnitQuaternion = abs(magnitude - 1.0) < epsilon;

    // if (!isUnitQuaternion)
    // {
    //     q.normalize();
    // }

    rotMatEuler = q2R(q);

    float test = -rotMatEuler(2, 0);

    if (test > 0.99999)
    {
        eularAngle(2) = 0.0f;
        eularAngle(1) = PI / 2;
        eularAngle(0) = atan2(rotMatEuler(0, 1), rotMatEuler(0, 2));
    }
    else if (test < -0.99999)
    {
        eularAngle(2) = 0.0f;
        eularAngle(1) = -PI / 2;
        eularAngle(0) = atan2(-rotMatEuler(0, 1), -rotMatEuler(0, 2));
    }
    else
    {
        eularAngle(2) = atan2(rotMatEuler(1, 0), rotMatEuler(0, 0));
        eularAngle(1) = asin(-rotMatEuler(2, 0));
        eularAngle(0) = atan2(rotMatEuler(2, 1), rotMatEuler(2, 2));
    }
}
2

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.