Struggle to get position at 50hz using extended kalman filter using imu and gps sensor

[jimit007]

Hi, jimit here, I am experiencing struggle to get 50 hz position and velocity using imu and gps sensor, i use imu bmi270 and m8n gps, imu sensor has 50hz frequency and gps has 10hz frequency, i give you my ekf code, i built seprate algorithm for attitude estimation using quarternion, I have questions any preprocessing required to synchronise imu and gps data or ekf automatically do for me?

Here is my code

#include <INS/stateEstimation.h>

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

void stateEstimation::InitializeFilter()
{
    prosessGpsInit();

    gravity << 0.0f, 0.0f, -9.80665f;

    proccessNoice(0, 0) = 0.2;
    proccessNoice(1, 1) = 0.2;
    proccessNoice(2, 2) = 0.3;

    P.block(0, 0, 3, 3) = pow(P_V_INIT, 2.0) * Eigen::Matrix<float, 3, 3>::Identity();
    P.block(3, 3, 3, 3) = pow(P_P_INIT, 2.0) * Eigen::Matrix<float, 3, 3>::Identity();

    R.block(0, 0, 2, 2) = pow(SIG_GPS_V_NE, 2.0) * Eigen::Matrix<float, 2, 2>::Identity();
    R(2, 2) = pow(SIG_GPS_V_D, 2.0);
    R.block(3, 3, 2, 2) = pow(SIG_GPS_P_NE, 2.0) * Eigen::Matrix<float, 2, 2>::Identity();
    R(5, 5) = pow(SIG_GPS_P_D, 2.0);

    currentTime = micros();
}

void stateEstimation::getRotMat(Vector4f q)
{
    float q0 = q(0), q1 = q(1), q2 = q(2), q3 = q(3);

    rotMat(0, 0) = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
    rotMat(0, 1) = 2.0f * (q1 * q2 - q0 * q3);
    rotMat(0, 2) = 2.0f * (q1 * q3 + q0 * q2);

    rotMat(1, 0) = 2.0f * (q1 * q2 + q0 * q3);
    rotMat(1, 1) = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
    rotMat(1, 2) = 2.0f * (q2 * q3 - q0 * q1);

    rotMat(2, 0) = 2.0f * (q1 * q3 - q0 * q2);
    rotMat(2, 1) = 2.0f * (q2 * q3 + q0 * q1);
    rotMat(2, 2) = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
}

void stateEstimation::Update(float dt, Vector4f q, Vector3f accel)
{
    getRotMat(q);

    statePrev = state;

    lpf_accel = 0.85f * lpf_accel + accel * 0.15f;

    delVel = (rotMat * lpf_accel) + gravity;

    accNavMag = delVel.norm();

    state(0) = state(0) + delVel(0) * dt;
    state(1) = state(1) + delVel(1) * dt;
    state(2) = state(2) + delVel(2) * dt;

    state(3) = state(3) + 0.5f * (state(0) + statePrev(0)) * dt;
    state(4) = state(4) + 0.5f * (state(1) + statePrev(1)) * dt;
    state(5) = state(5) + 0.5f * (state(2) + statePrev(2)) * dt;

    for (size_t i = 0; i <= 2; i++)
    {
        state[i] = constrain(state[i], -5.0e2f, 5.0e2f);
    }

    // Constrain position to a reasonable vehicle range (in meters)
    for (size_t i = 3; i <= 4; i++)
    {
        state[i] = constrain(state[i], -1.0e6f, 1.0e6f);
    }

    state[5] = constrain(state[5], -4.0e4f, 4.0e4f);

    upDateCov(dt);

    fuseGpsData(dt);

    double posN = state(3);
    double posE = state(4);
    float posD = state(5);

    converter.ned2Geodetic(posN, posE, posD, &newLatitiude, &newLongitude, &newAltitude);
    newLocation(0) = newLatitiude;
    newLocation(1) = newLongitude;
    newLocation(2) = newAltitude;
}

void stateEstimation::upDateCov(float dt)
{
    Fs.setIdentity();
    Fs(3, 0) = dt;
    Fs(4, 1) = dt;
    Fs(5, 2) = dt;

    Q(0, 0) = powf(proccessNoice(0, 0) * dt, 2);
    Q(1, 1) = powf(proccessNoice(1, 1) * dt, 2);
    Q(2, 2) = powf(proccessNoice(2, 2) * dt, 2);

    Q(3, 3) = powf(proccessNoice(0, 0) * (dt * dt), 2);
    Q(4, 4) = powf(proccessNoice(1, 1) * (dt * dt), 2);
    Q(5, 5) = powf(proccessNoice(2, 2) * (dt * dt), 2);

    // Q = 0.5f * (Q + Q.transpose());

    P = Fs * P * Fs.transpose() + Q;
    P = 0.5f * (P + P.transpose());

    // Constrain velocity variances
    for (size_t i = 0; i <= 2; i++)
    {
        P(i, i) = constrain(P(i, i), 0.0f, 1.0e3f);
    }

    // Constrain position variances
    for (size_t i = 3; i <= 5; i++)
    {
        P(i, i) = constrain(P(i, i), 0.0f, 1.0e6f);
    }
}

void stateEstimation::fuseData(Vector4f q, Vector3f accel)
{
    if (GPS_ADD_COUNTER >= 0)
        GPS_ADD_COUNTER--;

    GpsDataAvailable = gps.processGPS();

    if (GpsDataAvailable == gps.MT_NAV_PVT)
    {

        if ((byte)gps.ubxMessage.navPvt.fixType > 0)
        {
            SignalAvailable = true;

            Lat = (double)gps.ubxMessage.navPvt.lat * 1e-07f;
            Lon = (double)gps.ubxMessage.navPvt.lon * 1e-07f;
            altitude = (float)gps.ubxMessage.navPvt.height * 1e-03f;

            VEL_N = (float)gps.ubxMessage.navPvt.velN * 1e-03f;
            VEL_E = (float)gps.ubxMessage.navPvt.velE * 1e-03f;
            VEL_D = (float)gps.ubxMessage.navPvt.velD * 1e-03f;
            NUM_OF_SAT = (int8_t)gps.ubxMessage.navPvt.numSV;
            converter.geodetic2Ned(Lat, Lon, (float)altitude, &north, &east, &down);

            actual(0) = VEL_N;
            actual(1) = VEL_E;
            actual(2) = VEL_D;

            actual(3) = north;
            actual(4) = east;
            actual(5) = down;

            GPS_ADD_COUNTER = 5;
            NEW_GPS_DATA_COUNTER = 9;
            NEW_GPS_DATA_AVAILABLE = 1;
        }
        else
        {
            digitalWrite(13, !digitalRead(13));
            actual(0) = 0.0f;
            actual(1) = 0.0f;
            actual(2) = 0.0f;

            actual(3) = 0.0f;
            actual(4) = 0.0f;
            actual(5) = 0.0f;
            digitalWrite(40, !digitalRead(40));
            NUM_OF_SAT = 0;
        }
    }

    if (GPS_ADD_COUNTER == 0 && NEW_GPS_DATA_COUNTER > 0)
    {                               // If gps_add_counter is 0 and there are new GPS simulations needed.
        NEW_GPS_DATA_AVAILABLE = 1; // Set the new_gps_data_available to indicate that there is new data available.
        NEW_GPS_DATA_COUNTER--;     // Decrement the new_gps_data_counter so there will only be 9 simulations
        GPS_ADD_COUNTER = 5;        // Set the gps_add_counter variable to 5 as a count down loop timer
    }

    // this bolow code run at 50Hz.

    if (NEW_GPS_DATA_AVAILABLE)
    {
        if ((byte)gps.ubxMessage.navPvt.numSV < 8)
        {
            digitalWrite(13, !digitalRead(13));
            digitalWrite(40, !digitalRead(40));
        }
        else
        {
            digitalWrite(13, LOW);
            digitalWrite(40, LOW);
        }

        previousTime = currentTime;
        currentTime = micros();
        dtGPS = (float)(currentTime - previousTime) / 1000000.0f;

        Update(dtGPS, q, accel);
        NEW_GPS_DATA_AVAILABLE = 0;

        GPS_LOOP = true;
    }
}

void stateEstimation::fuseGpsData(float dt)
{
    GpsDataAvailable = gps.processGPS();

    if (GpsDataAvailable == gps.MT_NAV_PVT)
    {
        previousTime = currentTime;
        currentTime = (float)micros();
        dtGPS = (currentTime - previousTime) / 1000000.0f;

        filteredGPSdt = filteredGPSdt * 0.95f + dtGPS * 0.05f;

        if ((byte)gps.ubxMessage.navPvt.fixType > 0)
        {

            Lat = (double)gps.ubxMessage.navPvt.lat * 1e-07f;
            Lon = (double)gps.ubxMessage.navPvt.lon * 1e-07f;
            altitude = (float)gps.ubxMessage.navPvt.height * 1e-03f;

            VEL_N = (float)gps.ubxMessage.navPvt.velN * 1e-03f;
            VEL_E = (float)gps.ubxMessage.navPvt.velE * 1e-03f;
            VEL_D = (float)gps.ubxMessage.navPvt.velD * 1e-03f;
            NUM_OF_SAT = (int8_t)gps.ubxMessage.navPvt.numSV;
            converter.geodetic2Ned(Lat, Lon, (float)altitude, &north, &east, &down);

            actual(0) = VEL_N;
            actual(1) = VEL_E;
            actual(2) = VEL_D;

            actual(3) = north;
            actual(4) = east;
            actual(5) = down;

            error = actual - state;

            float gpsVarianceScaler = filteredGPSdt / dt;
            float velErr = 0.2f * accNavMag; // additional error in GPS velocities caused by manoeuvring
            float posErr = 0.2f * accNavMag; // additional error in GPS position caused by manoeuvring

            R(0, 0) = gpsVarianceScaler * powf(0.02, 2) + powf(velErr, 2);
            R(1, 1) = R(0, 0);
            R(2, 2) = gpsVarianceScaler * powf(0.006, 2) + powf(velErr, 2);
            R(3, 3) = gpsVarianceScaler * powf(0.003, 2) + powf(posErr, 2);
            R(4, 4) = R(3, 3);
            R(5, 5) = gpsVarianceScaler * powf(0.0007, 2) + powf(posErr, 2);

            S = H * P * H.transpose() + R;
            K = P * H.transpose() * S.inverse();

            state = state + (K * error);

            P = P - (K * H * P);
            P = 0.5f * (P + P.transpose());

            // Serial.println(state(3));
        }
        else
        {
            digitalWrite(13, !digitalRead(13));
            actual(0) = 0.0f;
            actual(1) = 0.0f;
            actual(2) = 0.0f;

            actual(3) = 0.0f;
            actual(4) = 0.0f;
            actual(5) = 0.0f;
            digitalWrite(40, !digitalRead(40));
            NUM_OF_SAT = 0;
        }
    }
}

void stateEstimation::prosessGpsInit()
{
    gps.gps_setup();
    delay(4);

    Lat = 0.0;
    Lon = 0.0;
    altitude = 0.0;

    VEL_N = 0.0;
    VEL_E = 0.0;
    VEL_D = 0.0;

    while (Lat == 0 && Lon == 0.0)
    {
        GpsDataAvailable = gps.processGPS();
        if (GpsDataAvailable == gps.MT_NAV_PVT)
        {

            Lat = gps.ubxMessage.navPvt.lat * 1e-07;
            Lon = gps.ubxMessage.navPvt.lon * 1e-07;
            altitude = gps.ubxMessage.navPvt.height * 1e-03;

            VEL_N = gps.ubxMessage.navPvt.velN * 1e-03;
            VEL_E = gps.ubxMessage.navPvt.velE * 1e-03;
            VEL_D = gps.ubxMessage.navPvt.velD * 1e-03;
            digitalWrite(13, !digitalRead(13));
            digitalWrite(40, !digitalRead(40));
        }

        delay(20);
    }

    converter = GeodeticConverter(Lat, Lon, (float)altitude);
    converter.geodetic2Ned(Lat, Lon, (float)altitude, &north, &east, &down);

    state(3) = north;
    state(4) = east;
    state(5) = down;

    state(0) = VEL_N;
    state(1) = VEL_E;
    state(2) = VEL_D;
}

#include <Arduino.h>
#include <ArduinoEigenDense.h>
#include <GPS/Geo.h>
#include <GPS/GPS.h>

class stateEstimation
{
private:
    /* data */
public:
    stateEstimation(/* args */);
    GPS gps = GPS();
    GeodeticConverter converter;

    int8_t GpsDataAvailable;
    bool isAvailable;
    double north, east, down;
    double Lat, Lon;
    double VEL_N, VEL_E, VEL_D;
    float altitude, accNavMag, newAltitude;
    double newLatitiude, newLongitude;

    double SIG_GPS_P_NE = 0.003; // 3.0
    double SIG_GPS_P_D = 0.0007;
    double P_P_INIT = 12.0;
    double P_V_INIT = 1.0;

    // GPS measurement noise std dev (m/s)
    double SIG_GPS_V_NE = 0.02;
    double SIG_GPS_V_D = 0.006;

    VectorXf state = VectorXf::Zero(6);
    VectorXf statePrev = VectorXf::Zero(6);
    VectorXf stateNew = VectorXf::Zero(6);

    Vector3f delVel = Vector3f::Zero();
    Vector3f gravity = Vector3f::Zero();

    MatrixXf P = MatrixXf::Identity(6, 6);
    MatrixXf pHat = MatrixXf::Identity(6, 6);
    MatrixXf pNew = MatrixXf::Identity(6, 6);

    MatrixXf Q = MatrixXf::Identity(6, 6);
    MatrixXf Fs = MatrixXf::Identity(6, 6);
    MatrixXf H = MatrixXf::Identity(6, 6);

    MatrixXf R = MatrixXf::Zero(6, 6);
    MatrixXf K = MatrixXf::Zero(6, 6);
    MatrixXf S = MatrixXf::Zero(6, 6);

    Matrix3f proccessNoice = Matrix3f::Identity();
    Matrix3f rotMat = Matrix3f::Zero();

    Vector3d newLocation = Vector3d::Zero();
    Vector3f lpf_accel = Vector3f::Zero();

    void Update(float dt, Vector4f q, Vector3f accel);
    void upDateCov(float dt);
    void getRotMat(Vector4f q);
    void fuseGpsData(float dt);
    void fuseData(Vector4f q, Vector3f accel);
    void InitializeFilter();
    void prosessGpsInit();

    void setPosition()
    {
        GpsDataAvailable = gps.processGPS();
        if (GpsDataAvailable == gps.MT_NAV_PVT)
        {

            Lat = gps.ubxMessage.navPvt.lat * 1e-07;
            Lon = gps.ubxMessage.navPvt.lon * 1e-07;
            altitude = gps.ubxMessage.navPvt.height * 1e-03;

            VEL_N = gps.ubxMessage.navPvt.velN * 1e-03;
            VEL_E = gps.ubxMessage.navPvt.velE * 1e-03;
            VEL_D = gps.ubxMessage.navPvt.velD * 1e-03;
            digitalWrite(13, !digitalRead(13));
            digitalWrite(40, !digitalRead(40));
        }

        converter.setHome(Lat, Lon, altitude);
        converter.geodetic2Ned(Lat, Lon, (float)altitude, &north, &east, &down);

        state(3) = north;
        state(4) = east;
        state(5) = down;

        state(0) = VEL_N;
        state(1) = VEL_E;
        state(2) = VEL_D;
    }

    VectorXf error = VectorXf::Zero(6);
    VectorXf actual = VectorXf::Zero(6);

    int GPS_ADD_COUNTER = 0;
    int NEW_GPS_DATA_COUNTER = 0;
    int NEW_GPS_DATA_AVAILABLE = 0;
    byte NUM_OF_SAT;
    bool SignalAvailable = false, GPS_LOOP = false;

    float currentTime, previousTime;
    float dtGPS, filteredGPSdt;
};

Here how I use this algorithm

prevTimeIMU = currentTimeIMU;
currentTimeIMU = (float)micros();
dtIMU = (currentTimeIMU - prevTimeIMU) / 1000000.0f;
dtIMUFiltered = 0.85 * dtIMUFiltered + 0.15 * dtIMU;


estimation.Update(dtIMUFiltered, ekf.xhatNew, mAcc);

Ekf.xHatNew are the quarternion to get rotation matrix inside update function.

Assume that gps library i already built which give me 10hz location, and velocity data using ublox protocol and i already converted gps lat, lon to ned frame, you can already see, but I am struggling to get stable 50hz position estimation, i predict at 50hz and update at 10hz, I am not getting accurate results, can you help me friends, I am frustrated last 2 week, I spend two week for this algorithm, if I increase process noise i fase to much delay and if I decrease process noise I am not getting perfect 50hz data what should I do?

[jremington]

@jimit007

I am experiencing struggle to get 50 hz position and velocity using imu

Everyone does. This topic comes up once a week or so on the forum, and to my knowledge no one has reported success.

Fusing GPS and accurate IMU data can be done, but in general, consumer grade IMUs are simply too inaccurate and too noisy to provide useful velocity and position information.

As this blog post explains, one of the really major challenges is accurately predicting the orientation, so that one can accurately subtract the gravity vector from the total acceleration.

An orientation error of even 0.1 degree is generally fatal, and you will spend $10K to $50K for an IMU capable of achieving higher accuracy than that.

[jimit007]

so, brother, how PX4 and ardupilot can do it successfully get a position in a 50Hz loop? That means it is possible, but we need to know how! I know we do not get much accuracy but we are at list close!

[jim-p]

See this post:

Problem in design position controller!

Hi! Jimit here! I am working with a position controller for my drone, I use EKF for position estimation, and my loop was 50Hz, I designed a controller 1Khz rate loop, 250Hz Angle Controller loop, 100Hz Altitude Hold using baro, and posHold 50Hz loop, I have a question about how I combine roll, pitch setpoint to position controller? I use an "NED" frame, I give you the code to my GPS pos hold. can you guide me, am I doing right? All sensors are properly calibrated, and the controller work awesom…

[jimit007]

@jim-p

See this post:

Sir, I need a solution! I spent too much time on both problems! My EKF gives me accuracy but at a low rate other hand I have dought, about my maths! so I make another post about position controllers both topics are different one for state estimation and the other for controllers!

[jremington]

@jimit007

ardupilot can do it successfully get a position in a 50Hz loop?

I wonder what you think "it" is.

The Ardupilot code is open source, so read the code.

[jimit007]

sure bro, If I get the solution I post it here, so everyone uses it! thank you brother!

[micahfriesen]

Why do you need a 50hz position rate?

[jimit007]

I design position controller, To stable my control loop, minimum need that frequency .