uNav INS

Maybe there a way to use the uNavINS without using the ublox library, using TinyGPS or similar to retrieve the data required by the INS.
The problem is that that library doesn't has the way to retrieve the NED velocity in all axes.
Ill try my 6M without using the ublox library.


mjs513 said:
Not sure about that one only @brtaylor can answer that one.
Click to expand...
 
Maybe there a way to use the uNavINS without using the ublox library, using TinyGPS or similar to retrieve the data required by the INS.
The problem is that that library doesn't has the way to retrieve the NED velocity in all axes.
Ill try my 6M without using the ublox library.


mjs513 said:
Not sure about that one only @brtaylor can answer that one.
Click to expand...
 
mjs513 said:
Not sure about that one only @brtaylor can answer that one.
Click to expand...

I doubt that my library will work with a 6M receiver. I test with a ZED-F9P and a SAM-M8Q. IIRC, there was a change in UBX-NAV-PVT packet size between the uBlox 6 and uBlox 8. I have an old uBlox 6 or 7, but won't have a chance to test it until next week.

Other libraries should work, but you do need good LLH position and NED velocity. Also, might need to check units, IIRC LLH should be in rad and meter and NED velocity should be in m/s. SparkFun has a good uBlox library, but I'm not sure if it would support a 6M.
 
I tried the code with the TinyGPS library, which works with 6M receivers, and each value using the specific units you and the repository says but im getting nan's values after a few seconds of running the program.

Something i tried was to use only the NED velocity components and put zero on the latitude and longitude inputs, and it works fine the same way as when the GPS is disabled, but i think that could alter the estimation.

Lat and long are in rad. NED velocities in m/s and altitude in meters.

brtaylor said:
I doubt that my library will work with a 6M receiver. I test with a ZED-F9P and a SAM-M8Q. IIRC, there was a change in UBX-NAV-PVT packet size between the uBlox 6 and uBlox 8. I have an old uBlox 6 or 7, but won't have a chance to test it until next week.

Other libraries should work, but you do need good LLH position and NED velocity. Also, might need to check units, IIRC LLH should be in rad and meter and NED velocity should be in m/s. SparkFun has a good uBlox library, but I'm not sure if it would support a 6M.
Click to expand...
 
I just want to make sure that the problem is or not the GPS receiver before getting another one, because im using the correct units, and its a problem that we had before.

mjs513 said:
Not sure about that one only @brtaylor can answer that one.
Click to expand...
 
The problem we had before was related to the deltaT being incorrect. Once that was fixed it there should be no issues associated with the EKF unless your BNO055 or GPS data is corrupted.
 
Ive tried your code with a brand new 8M gps module (Radiolink SE100) and im getting the same nan's error. Don't know what is happening because the EKF works fine without the GPS input, once i activate de GPS, it alters the values until it gets nans.
Theres something with the GPS input that is creating issues.

Code: 
Filter.update(gnss.gps_tow_s(),gnss.north_vel_mps(),
                gnss.east_vel_mps(),gnss.down_vel_mps(),
                gnss.lat_rad(),gnss.lon_rad(),gnss.alt_msl_m(), 
                gyro_x*PI/180,gyro_y*PI/180,gyro_z*PI/180,         //deg/s to rad/s
                acc_x,acc_y,acc_z,
                mag_x,mag_y,mag_z);

Im retrieving each variable after the filter update to debug each value and it shows every GPS value right.

Code: 
GPS 8M     | FIX STATUS: 2 SATS: 5 LATITUDE: [HOME LAT :rolleyes: in deg] LONGITUDE: [HOME LON :rolleyes: in deg] GPS ALTITUDE: 1815.91
	   | GROUND SPEED m/s: 0.34 GROUND SPEED km/h: 1.21 GROUND SPEED knots: 1.21
	   | NORTH SPEED m/s: 0.34 EAST SPEED m/s: 0.02 DOWN SPEED m/s: 0.82 GPS HEADING: 8.97
	   | DATE: 0/0/0 - 0/0/0
	   | TOW: 279173.20

Obviously im testing this inside my apartment, but i get the same result with 10 sats or more.

I don't think the issue is the BNO055 because the EKF works perfect when declare tow=0 to avoid GPS updating, and the GPS is retrieving the data directly from the ubx library.

Maybe the problem is into the uNavINS.h or uNavINS.cpp.

here both:

uNavINS.h
Code: 
/*
uNavINS.h

Original Author:
Adhika Lie
2012-10-08
University of Minnesota
Aerospace Engineering and Mechanics
Copyright 2011 Regents of the University of Minnesota. All rights reserved.

Updated to be a class, use Eigen, and compile as an Arduino library.
Added methods to get gyro and accel bias. Added initialization to
estimated angles rather than assuming IMU is level. Added method to get psi,
rather than just heading, and ground track.
Brian R Taylor
brian.taylor@bolderflight.com
2017-12-20
Bolder Flight Systems
Copyright 2017 Bolder Flight Systems

Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or
substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

// XXX - accel and gyro bias not being updated.
// XXX - add set methods for sensor characteristics.
// XXX - is psi heading or track? I think heading.
// XXX - add outputs for filter covariance to measure convergence.
// XXX - incorporate magnetometers.


#ifndef UNAVINS_H
#define UNAVINS_H
#if defined(ARDUINO)
  #include "Arduino.h"
  #include "eigen.h"
  #include <Eigen/Dense>
#else
  #include <sys/time.h>
  #include <stdint.h>
  #include <math.h>
  #include <Eigen/Core>
  #include <Eigen/Dense>

  uint64_t micros() {
      struct timeval tv;
      gettimeofday(&tv,NULL);
      return tv.tv_sec*(uint64_t)1000000+tv.tv_usec;
  }

  class elapsedMicros
  {
  private:
  	unsigned long us;
  public:
  	elapsedMicros(void) { us = micros(); }
  	elapsedMicros(unsigned long val) { us = micros() - val; }
  	elapsedMicros(const elapsedMicros &orig) { us = orig.us; }
  	operator unsigned long () const { return micros() - us; }
  	elapsedMicros & operator = (const elapsedMicros &rhs) { us = rhs.us; return *this; }
  	elapsedMicros & operator = (unsigned long val) { us = micros() - val; return *this; }
  	elapsedMicros & operator -= (unsigned long val)      { us += val ; return *this; }
  	elapsedMicros & operator += (unsigned long val)      { us -= val ; return *this; }
  	elapsedMicros operator - (int val) const           { elapsedMicros r(*this); r.us += val; return r; }
  	elapsedMicros operator - (unsigned int val) const  { elapsedMicros r(*this); r.us += val; return r; }
  	elapsedMicros operator - (long val) const          { elapsedMicros r(*this); r.us += val; return r; }
  	elapsedMicros operator - (unsigned long val) const { elapsedMicros r(*this); r.us += val; return r; }
  	elapsedMicros operator + (int val) const           { elapsedMicros r(*this); r.us -= val; return r; }
  	elapsedMicros operator + (unsigned int val) const  { elapsedMicros r(*this); r.us -= val; return r; }
  	elapsedMicros operator + (long val) const          { elapsedMicros r(*this); r.us -= val; return r; }
  	elapsedMicros operator + (unsigned long val) const { elapsedMicros r(*this); r.us -= val; return r; }
  };
#endif

class uNavINS {
  public:
    void update(unsigned long TOW,double vn,double ve,double vd,double lat,double lon,double alt,float p,float q,float r,float ax,float ay,float az,float hx,float hy, float hz);
    float getPitch_rad();
    float getRoll_rad();
    float getYaw_rad();
    float getHeading_rad();
    double getLatitude_rad();
    double getLongitude_rad();
    double getAltitude_m();
    double getVelNorth_ms();
    double getVelEast_ms();
    double getVelDown_ms();
    float getGroundTrack_rad();
    float getGyroBiasX_rads();
    float getGyroBiasY_rads();
    float getGyroBiasZ_rads();
    float getAccelBiasX_mss();
    float getAccelBiasY_mss();
    float getAccelBiasZ_mss();
  private:
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // error characteristics of navigation parameters
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Std dev of Accelerometer Wide Band Noise (m/s^2)
    const float SIG_W_A = 1.0f;       // 1 m/s^2
    // Std dev of gyro output noise (rad/s)
    const float SIG_W_G = 0.00524f;   // 0.3 deg/s
    // Std dev of Accelerometer Markov Bias
    const float SIG_A_D = 0.1f;       // 5e-2*g
    // Correlation time or time constant
    const float TAU_A = 100.0f;
    // Std dev of correlated gyro bias
    const float SIG_G_D = 0.00873f;   // 0.1 deg/s
    // Correlation time or time constant
    const float TAU_G = 50.0f;
    // GPS measurement noise std dev (m)
    const float SIG_GPS_P_NE = 3.0f;
    const float SIG_GPS_P_D = 5.0f;
    // GPS measurement noise std dev (m/s)
    const float SIG_GPS_V = 0.5f;
    // Initial set of covariance
    const float P_P_INIT = 10.0f;
    const float P_V_INIT = 1.0f;
    const float P_A_INIT = 0.34906f;     // 20 deg
    const float P_HDG_INIT = 3.14159f;   // 180 deg
    const float P_AB_INIT = 0.9810f;     // 0.5*g
    const float P_GB_INIT = 0.01745f;    // 5 deg/s
    // acceleration due to gravity
    const float G = 9.807f;
    // major eccentricity squared
    const double ECC2 = 0.0066943799901;
    // earth semi-major axis radius (m)
    const double EARTH_RADIUS = 6378137.0;
    // initialized
    bool initialized = false;
    // timing
    elapsedMicros _t;
    float _dt;
    unsigned long previousTOW;
    // estimated attitude
    float phi, theta, psi, heading;
    // initial heading angle
    float psi_initial;
    // estimated NED velocity
    double vn_ins, ve_ins, vd_ins;
    // estimated location
    double lat_ins, lon_ins, alt_ins;
    // magnetic heading corrected for roll and pitch angle
    float Bxc, Byc;
    // accelerometer bias
    float abx, aby, abz;
    // gyro bias
    float gbx, gby, gbz;
    // earth radius at location
    double Re, Rn, denom;
    // State matrix
    Eigen::Matrix<float,15,15> Fs = Eigen::Matrix<float,15,15>::Identity();
    // State transition matrix
    Eigen::Matrix<float,15,15> PHI = Eigen::Matrix<float,15,15>::Zero();
    // Covariance matrix
    Eigen::Matrix<float,15,15> P = Eigen::Matrix<float,15,15>::Zero();
    // For process noise transformation
    Eigen::Matrix<float,15,12> Gs = Eigen::Matrix<float,15,12>::Zero();
    Eigen::Matrix<float,12,12> Rw = Eigen::Matrix<float,12,12>::Zero();
    // Process noise matrix
    Eigen::Matrix<float,15,15> Q = Eigen::Matrix<float,15,15>::Zero();
    // Gravity model
    Eigen::Matrix<float,3,1> grav = Eigen::Matrix<float,3,1>::Zero();
    // Rotation rate
    Eigen::Matrix<float,3,1> om_ib = Eigen::Matrix<float,3,1>::Zero();
    // Specific force
    Eigen::Matrix<float,3,1> f_b = Eigen::Matrix<float,3,1>::Zero();
    // DCM
    Eigen::Matrix<float,3,3> C_N2B = Eigen::Matrix<float,3,3>::Zero();
    // DCM transpose
    Eigen::Matrix<float,3,3> C_B2N = Eigen::Matrix<float,3,3>::Zero();
    // Temporary to get dxdt
    Eigen::Matrix<float,3,1> dx = Eigen::Matrix<float,3,1>::Zero();
    Eigen::Matrix<double,3,1> dxd = Eigen::Matrix<double,3,1>::Zero();
    // NED velocity INS
    Eigen::Matrix<double,3,1> V_ins = Eigen::Matrix<double,3,1>::Zero();
    // LLA INS
    Eigen::Matrix<double,3,1> lla_ins = Eigen::Matrix<double,3,1>::Zero();
    // NED velocity GPS
    Eigen::Matrix<double,3,1> V_gps = Eigen::Matrix<double,3,1>::Zero();
    // LLA GPS
    Eigen::Matrix<double,3,1> lla_gps = Eigen::Matrix<double,3,1>::Zero();
    // Position ECEF INS
    Eigen::Matrix<double,3,1> pos_ecef_ins = Eigen::Matrix<double,3,1>::Zero();
    // Position NED INS
    Eigen::Matrix<double,3,1> pos_ned_ins = Eigen::Matrix<double,3,1>::Zero();
    // Position ECEF GPS
    Eigen::Matrix<double,3,1> pos_ecef_gps = Eigen::Matrix<double,3,1>::Zero();
    // Position NED GPS
    Eigen::Matrix<double,3,1> pos_ned_gps = Eigen::Matrix<double,3,1>::Zero();
    // Quat
    Eigen::Matrix<float,4,1> quat = Eigen::Matrix<float,4,1>::Zero();
    // dquat
    Eigen::Matrix<float,4,1> dq = Eigen::Matrix<float,4,1>::Zero();
    // difference between GPS and INS
    Eigen::Matrix<float,6,1> y = Eigen::Matrix<float,6,1>::Zero();
    // GPS measurement noise
    Eigen::Matrix<float,6,6> R = Eigen::Matrix<float,6,6>::Zero();
    Eigen::Matrix<float,15,1> x = Eigen::Matrix<float,15,1>::Zero();
    // Kalman Gain
    Eigen::Matrix<float,15,6> K = Eigen::Matrix<float,15,6>::Zero();
    Eigen::Matrix<float,6,15> H = Eigen::Matrix<float,6,15>::Zero();
    // skew symmetric
    Eigen::Matrix<float,3,3> sk(Eigen::Matrix<float,3,1> w);
    // lla rate
    Eigen::Matrix<double,3,1> llarate(Eigen::Matrix<double,3,1> V,Eigen::Matrix<double,3,1> lla);
    // lla to ecef
    Eigen::Matrix<double,3,1> lla2ecef(Eigen::Matrix<double,3,1> lla);
    // ecef to ned
    Eigen::Matrix<double,3,1> ecef2ned(Eigen::Matrix<double,3,1> ecef,Eigen::Matrix<double,3,1> pos_ref);
    // quaternion to dcm
    Eigen::Matrix<float,3,3> quat2dcm(Eigen::Matrix<float,4,1> q);
    // quaternion multiplication
    Eigen::Matrix<float,4,1> qmult(Eigen::Matrix<float,4,1> p, Eigen::Matrix<float,4,1> q);
    // maps angle to +/- 180
    float constrainAngle180(float dta);
    // maps angle to 0-360
    float constrainAngle360(float dta);
};

#endif

uNavINS.cpp

Code: 
/*
uNavINS.cpp

Original Author:
Adhika Lie
2012-10-08
University of Minnesota
Aerospace Engineering and Mechanics
Copyright 2011 Regents of the University of Minnesota. All rights reserved.

Updated to be a class, use Eigen, and compile as an Arduino library.
Added methods to get gyro and accel bias. Added initialization to
estimated angles rather than assuming IMU is level. Added method to get psi,
rather than just heading, and ground track.
Brian R Taylor
brian.taylor@bolderflight.com
2017-12-20
Bolder Flight Systems
Copyright 2017 Bolder Flight Systems

Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute,
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or
substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include "uNavINS.h"

void uNavINS::update(unsigned long TOW,double vn,double ve,double vd,double lat,double lon,double alt,float p,float q,float r,float ax,float ay,float az,float hx,float hy, float hz) {
  if (!initialized) {
    // initial attitude and heading
    theta = asinf(ax/G);
    phi = asinf(-ay/(G*cosf(theta)));
    // magnetic heading correction due to roll and pitch angle
    Bxc = hx*cosf(theta) + (hy*sinf(phi) + hz*cosf(phi))*sinf(theta);
    Byc = hy*cosf(phi) - hz*sinf(phi);
    // finding initial heading
    if (-Byc > 0) {
      psi = M_PI/2.0f - atanf(Bxc/-Byc);
    } else {
      psi= 3.0f*M_PI/2.0f - atanf(Bxc/-Byc);
    }
    psi = constrainAngle180(psi);
    psi_initial = psi;
    // euler to quaternion
    quat(0) = cosf(psi/2.0f)*cosf(theta/2.0f)*cosf(phi/2.0f) + sinf(psi/2.0f)*sinf(theta/2.0f)*sinf(phi/2.0f);
    quat(1) = cosf(psi/2.0f)*cosf(theta/2.0f)*sinf(phi/2.0f) - sinf(psi/2.0f)*sinf(theta/2.0f)*cosf(phi/2.0f);
    quat(2) = cosf(psi/2.0f)*sinf(theta/2.0f)*cosf(phi/2.0f) + sinf(psi/2.0f)*cosf(theta/2.0f)*sinf(phi/2.0f);
    quat(3) = sinf(psi/2.0f)*cosf(theta/2.0f)*cosf(phi/2.0f) - cosf(psi/2.0f)*sinf(theta/2.0f)*sinf(phi/2.0f);
    // Assemble the matrices
    // ... gravity
    grav(2,0) = G;
    // ... H
    H.block(0,0,5,5) = Eigen::Matrix<float,5,5>::Identity();
    // // ... Rw
    // Rw.block(0,0,3,3) = powf(SIG_W_A,2.0f)*Eigen::Matrix<float,3,3>::Identity();
    // Rw.block(3,3,3,3) = powf(SIG_W_G,2.0f)*Eigen::Matrix<float,3,3>::Identity();
    // Rw.block(6,6,3,3) = 2.0f*powf(SIG_A_D,2.0f)/TAU_A*Eigen::Matrix<float,3,3>::Identity();
    // Rw.block(9,9,3,3) = 2.0f*powf(SIG_G_D,2.0f)/TAU_G*Eigen::Matrix<float,3,3>::Identity();
    // // ... P
    // P.block(0,0,3,3) = powf(P_P_INIT,2.0f)*Eigen::Matrix<float,3,3>::Identity();
    // P.block(3,3,3,3) = powf(P_V_INIT,2.0f)*Eigen::Matrix<float,3,3>::Identity();
    // P.block(6,6,2,2) = powf(P_A_INIT,2.0f)*Eigen::Matrix<float,2,2>::Identity();
    // P(8,8) = powf(P_HDG_INIT,2.0f);
    // P.block(9,9,3,3) = powf(P_AB_INIT,2.0f)*Eigen::Matrix<float,3,3>::Identity();
    // P.block(12,12,3,3) = powf(P_GB_INIT,2.0f)*Eigen::Matrix<float,3,3>::Identity();
    // // ... R
    // R.block(0,0,2,2) = powf(SIG_GPS_P_NE,2.0f)*Eigen::Matrix<float,2,2>::Identity();
    // R(2,2) = powf(SIG_GPS_P_D,2.0f);
    // R.block(3,3,3,3) = powf(SIG_GPS_V,2.0f)*Eigen::Matrix<float,3,3>::Identity();
    // ... Rw
    Rw(0,0) = SIG_W_A*SIG_W_A;          Rw(1,1) = SIG_W_A*SIG_W_A;            Rw(2,2) = SIG_W_A*SIG_W_A;
    Rw(3,3) = SIG_W_G*SIG_W_G;          Rw(4,4) = SIG_W_G*SIG_W_G;            Rw(5,5) = SIG_W_G*SIG_W_G;
    Rw(6,6) = 2.0f*SIG_A_D*SIG_A_D/TAU_A; Rw(7,7) = 2.0f*SIG_A_D*SIG_A_D/TAU_A;   Rw(8,8) = 2.0f*SIG_A_D*SIG_A_D/TAU_A;
    Rw(9,9) = 2.0f*SIG_G_D*SIG_G_D/TAU_G; Rw(10,10) = 2.0f*SIG_G_D*SIG_G_D/TAU_G; Rw(11,11) = 2.0f*SIG_G_D*SIG_G_D/TAU_G;
    // ... P
    P(0,0) = P_P_INIT*P_P_INIT;       P(1,1) = P_P_INIT*P_P_INIT;       P(2,2) = P_P_INIT*P_P_INIT;
    P(3,3) = P_V_INIT*P_V_INIT;       P(4,4) = P_V_INIT*P_V_INIT;       P(5,5) = P_V_INIT*P_V_INIT;
    P(6,6) = P_A_INIT*P_A_INIT;       P(7,7) = P_A_INIT*P_A_INIT;       P(8,8) = P_HDG_INIT*P_HDG_INIT;
    P(9,9) = P_AB_INIT*P_AB_INIT;     P(10,10) = P_AB_INIT*P_AB_INIT;   P(11,11) = P_AB_INIT*P_AB_INIT;
    P(12,12) = P_GB_INIT*P_GB_INIT;   P(13,13) = P_GB_INIT*P_GB_INIT;   P(14,14) = P_GB_INIT*P_GB_INIT;
    // ... R
    R(0,0) = SIG_GPS_P_NE*SIG_GPS_P_NE; R(1,1) = SIG_GPS_P_NE*SIG_GPS_P_NE; R(2,2) = SIG_GPS_P_D*SIG_GPS_P_D;
    R(3,3) = SIG_GPS_V*SIG_GPS_V;       R(4,4) = SIG_GPS_V*SIG_GPS_V;       R(5,5) = SIG_GPS_V*SIG_GPS_V;
    // .. then initialize states with GPS Data
    lat_ins = lat;
    lon_ins = lon;
    alt_ins = alt;
    vn_ins = vn;
    ve_ins = ve;
    vd_ins = vd;
    // specific force
    f_b(0,0) = ax;
    f_b(1,0) = ay;
    f_b(2,0) = az;
    /* initialize the time */
    _t = 0;
    // initialized flag
    initialized = true;
  } else {
    // get the change in time
    _dt = (float)_t/1e6;
    _t = 0;
    lla_ins(0,0) = lat_ins;
    lla_ins(1,0) = lon_ins;
    lla_ins(2,0) = alt_ins;
    V_ins(0,0) = vn_ins;
    V_ins(1,0) = ve_ins;
    V_ins(2,0) = vd_ins;
    // AHRS Transformations
    C_N2B = quat2dcm(quat);
    C_B2N = C_N2B.transpose();
    // Attitude Update
    dq(0) = 1.0f;
    dq(1) = 0.5f*om_ib(0,0)*_dt;
    dq(2) = 0.5f*om_ib(1,0)*_dt;
    dq(3) = 0.5f*om_ib(2,0)*_dt;
    quat = qmult(quat,dq);
    quat.normalize();
    // Avoid quaternion flips sign
    if (quat(0) < 0) {
      quat = -1.0f*quat;
    }
    // obtain euler angles from quaternion
    theta = asinf(-2.0f*(quat(1,0)*quat(3,0)-quat(0,0)*quat(2,0)));
    phi = atan2f(2.0f*(quat(0,0)*quat(1,0)+quat(2,0)*quat(3,0)),1.0f-2.0f*(quat(1,0)*quat(1,0)+quat(2,0)*quat(2,0)));
    psi = atan2f(2.0f*(quat(1,0)*quat(2,0)+quat(0,0)*quat(3,0)),1.0f-2.0f*(quat(2,0)*quat(2,0)+quat(3,0)*quat(3,0)));
    // Velocity Update
    dx = C_B2N*f_b + grav;
    vn_ins += _dt*dx(0,0);
    ve_ins += _dt*dx(1,0);
    vd_ins += _dt*dx(2,0);
    // Position Update
    dxd = llarate(V_ins,lla_ins);
    lat_ins += _dt*dxd(0,0);
    lon_ins += _dt*dxd(1,0);
    alt_ins += _dt*dxd(2,0);
    // Jacobian
    Fs.setZero();
    // ... pos2gs
    Fs.block(0,3,3,3) = Eigen::Matrix<float,3,3>::Identity();
    // ... gs2pos
    Fs(5,2) = -2.0f*G/EARTH_RADIUS;
    // ... gs2att
    Fs.block(3,6,3,3) = -2.0f*C_B2N*sk(f_b);
    // ... gs2acc
    Fs.block(3,9,3,3) = -C_B2N;
    // ... att2att
    Fs.block(6,6,3,3) = -sk(om_ib);
    // ... att2gyr
    Fs.block(6,12,3,3) = -0.5f*Eigen::Matrix<float,3,3>::Identity();
    // ... Accel Markov Bias
    Fs.block(9,9,3,3) = -1.0f/TAU_A*Eigen::Matrix<float,3,3>::Identity();
    Fs.block(12,12,3,3) = -1.0f/TAU_G*Eigen::Matrix<float,3,3>::Identity();
    // State Transition Matrix
    PHI = Eigen::Matrix<float,15,15>::Identity()+Fs*_dt;
    // Process Noise
    Gs.setZero();
    Gs.block(3,0,3,3) = -C_B2N;
    Gs.block(6,3,3,3) = -0.5f*Eigen::Matrix<float,3,3>::Identity();
    Gs.block(9,6,6,6) = Eigen::Matrix<float,6,6>::Identity();
    // Discrete Process Noise
    Q = PHI*_dt*Gs*Rw*Gs.transpose();
    Q = 0.5f*(Q+Q.transpose());
    // Covariance Time Update
    P = PHI*P*PHI.transpose()+Q;
    P = 0.5f*(P+P.transpose());

    // Gps measurement update
    if ((TOW - previousTOW) > 0) {
      previousTOW = TOW;
      lla_gps(0,0) = lat;
      lla_gps(1,0) = lon;
      lla_gps(2,0) = alt;
      V_gps(0,0) = vn;
      V_gps(1,0) = ve;
      V_gps(2,0) = vd;
      lla_ins(0,0) = lat_ins;
      lla_ins(1,0) = lon_ins;
      lla_ins(2,0) = alt_ins;
      V_ins(0,0) = vn_ins;
      V_ins(1,0) = ve_ins;
      V_ins(2,0) = vd_ins;
      // Position, converted to NED
      pos_ecef_ins = lla2ecef(lla_ins);
      pos_ned_ins = ecef2ned(pos_ecef_ins,lla_ins);
      pos_ecef_gps = lla2ecef(lla_gps);
      pos_ned_gps = ecef2ned(pos_ecef_gps,lla_ins);
      // Create measurement Y
      y(0,0) = (float)(pos_ned_gps(0,0) - pos_ned_ins(0,0));
      y(1,0) = (float)(pos_ned_gps(1,0) - pos_ned_ins(1,0));
      y(2,0) = (float)(pos_ned_gps(2,0) - pos_ned_ins(2,0));
      y(3,0) = (float)(V_gps(0,0) - V_ins(0,0));
      y(4,0) = (float)(V_gps(1,0) - V_ins(1,0));
      y(5,0) = (float)(V_gps(2,0) - V_ins(2,0));
      // Kalman gain
      K = P*H.transpose()*(H*P*H.transpose() + R).inverse();
      // Covariance update
      P = (Eigen::Matrix<float,15,15>::Identity()-K*H)*P*(Eigen::Matrix<float,15,15>::Identity()-K*H).transpose() + K*R*K.transpose();
      // State update
      x = K*y;
      denom = (1.0 - (ECC2 * pow(sin(lla_ins(0,0)),2.0)));
      denom = sqrt(denom*denom);
      Re = EARTH_RADIUS / sqrt(denom);
      Rn = EARTH_RADIUS*(1.0-ECC2) / denom*sqrt(denom);
      alt_ins = alt_ins - x(2,0);
      lat_ins = lat_ins + x(0,0) / (Re + alt_ins);
      lon_ins = lon_ins + x(1,0) / (Rn + alt_ins) / cos(lat_ins);
      vn_ins = vn_ins + x(3,0);
      ve_ins = ve_ins + x(4,0);
      vd_ins = vd_ins + x(5,0);
      // Attitude correction
      dq(0,0) = 1.0f;
      dq(1,0) = x(6,0);
      dq(2,0) = x(7,0);
      dq(3,0) = x(8,0);
      quat = qmult(quat,dq);
      quat.normalize();
      // obtain euler angles from quaternion
      theta = asinf(-2.0f*(quat(1,0)*quat(3,0)-quat(0,0)*quat(2,0)));
      phi = atan2f(2.0f*(quat(0,0)*quat(1,0)+quat(2,0)*quat(3,0)),1.0f-2.0f*(quat(1,0)*quat(1,0)+quat(2,0)*quat(2,0)));
      psi = atan2f(2.0f*(quat(1,0)*quat(2,0)+quat(0,0)*quat(3,0)),1.0f-2.0f*(quat(2,0)*quat(2,0)+quat(3,0)*quat(3,0)));
      abx = abx + x(9,0);
      aby = aby + x(10,0);
      abz = abz + x(11,0);
      gbx = gbx + x(12,0);
      gby = gby + x(13,0);
      gbz = gbz + x(14,0);
    }
    // Get the new Specific forces and Rotation Rate,
    // use in the next time update
    f_b(0,0) = ax - abx;
    f_b(1,0) = ay - aby;
    f_b(2,0) = az - abz;

    om_ib(0,0) = p - gbx;
    om_ib(1,0) = q - gby;
    om_ib(2,0) = r - gbz;
  }
}

// returns the pitch angle, rad
float uNavINS::getPitch_rad() {
  return theta;
}

// returns the roll angle, rad
float uNavINS::getRoll_rad() {
  return phi;
}

// returns the yaw angle, rad
float uNavINS::getYaw_rad() {
  return constrainAngle180(psi-psi_initial);
}

// returns the heading angle, rad
float uNavINS::getHeading_rad() {
  return constrainAngle360(psi);
}

// returns the INS latitude, rad
double uNavINS::getLatitude_rad() {
  return lat_ins;
}

// returns the INS longitude, rad
double uNavINS::getLongitude_rad() {
  return lon_ins;
}

// returns the INS altitude, m
double uNavINS::getAltitude_m() {
  return alt_ins;
}

// returns the INS north velocity, m/s
double uNavINS::getVelNorth_ms() {
  return vn_ins;
}

// returns the INS east velocity, m/s
double uNavINS::getVelEast_ms() {
  return ve_ins;
}

// returns the INS down velocity, m/s
double uNavINS::getVelDown_ms() {
  return vd_ins;
}

// returns the INS ground track, rad
float uNavINS::getGroundTrack_rad() {
  return atan2f((float)ve_ins,(float)vn_ins);
}

// returns the gyro bias estimate in the x direction, rad/s
float uNavINS::getGyroBiasX_rads() {
  return gbx;
}

// returns the gyro bias estimate in the y direction, rad/s
float uNavINS::getGyroBiasY_rads() {
  return gby;
}

// returns the gyro bias estimate in the z direction, rad/s
float uNavINS::getGyroBiasZ_rads() {
  return gbz;
}

// returns the accel bias estimate in the x direction, m/s/s
float uNavINS::getAccelBiasX_mss() {
  return abx;
}

// returns the accel bias estimate in the y direction, m/s/s
float uNavINS::getAccelBiasY_mss() {
  return aby;
}

// returns the accel bias estimate in the z direction, m/s/s
float uNavINS::getAccelBiasZ_mss() {
  return abz;
}

// This function gives a skew symmetric matrix from a given vector w
Eigen::Matrix<float,3,3> uNavINS::sk(Eigen::Matrix<float,3,1> w) {
  Eigen::Matrix<float,3,3> C;
  C(0,0) = 0.0f;    C(0,1) = -w(2,0); C(0,2) = w(1,0);
  C(1,0) = w(2,0);  C(1,1) = 0.0f;    C(1,2) = -w(0,0);
  C(2,0) = -w(1,0); C(2,1) = w(0,0);  C(2,2) = 0.0f;
  return C;
}

// This function calculates the rate of change of latitude, longitude, and altitude.
Eigen::Matrix<double,3,1> uNavINS::llarate(Eigen::Matrix<double,3,1> V,Eigen::Matrix<double,3,1> lla) {
  double Rew, Rns, denom;
  Eigen::Matrix<double,3,1> lla_dot;

  denom = (1.0 - (ECC2 * pow(sin(lla(0,0)),2.0)));
  denom = sqrt(denom*denom);

  Rew = EARTH_RADIUS / sqrt(denom);
  Rns = EARTH_RADIUS*(1.0-ECC2) / denom*sqrt(denom);

  lla_dot(0,0) = V(0,0)/(Rns + lla(2,0));
  lla_dot(1,0) = V(1,0)/((Rew + lla(2,0))*cos(lla(0,0)));
  lla_dot(2,0) = -V(2,0);

  return lla_dot;
}

// This function calculates the ECEF Coordinate given the Latitude, Longitude and Altitude.
Eigen::Matrix<double,3,1> uNavINS::lla2ecef(Eigen::Matrix<double,3,1> lla) {
  double Rew, denom;
  Eigen::Matrix<double,3,1> ecef;

  denom = (1.0 - (ECC2 * pow(sin(lla(0,0)),2.0)));
  denom = sqrt(denom*denom);

  Rew = EARTH_RADIUS / sqrt(denom);

  ecef(0,0) = (Rew + lla(2,0)) * cos(lla(0,0)) * cos(lla(1,0));
  ecef(1,0) = (Rew + lla(2,0)) * cos(lla(0,0)) * sin(lla(1,0));
  ecef(2,0) = (Rew * (1.0 - ECC2) + lla(2,0)) * sin(lla(0,0));

  return ecef;
}

// This function converts a vector in ecef to ned coordinate centered at pos_ref.
Eigen::Matrix<double,3,1> uNavINS::ecef2ned(Eigen::Matrix<double,3,1> ecef,Eigen::Matrix<double,3,1> pos_ref) {
  Eigen::Matrix<double,3,1> ned;
  ned(2,0)=-cos(pos_ref(0,0))*cos(pos_ref(1,0))*ecef(0,0)-cos(pos_ref(0,0))*sin(pos_ref(1,0))*ecef(1,0)-sin(pos_ref(0,0))*ecef(2,0);
  ned(1,0)=-sin(pos_ref(1,0))*ecef(0,0) + cos(pos_ref(1,0))*ecef(1,0);
  ned(0,0)=-sin(pos_ref(0,0))*cos(pos_ref(1,0))*ecef(0,0)-sin(pos_ref(0,0))*sin(pos_ref(1,0))*ecef(1,0)+cos(pos_ref(0,0))*ecef(2,0);
  return ned;
}

// quaternion to dcm
Eigen::Matrix<float,3,3> uNavINS::quat2dcm(Eigen::Matrix<float,4,1> q) {
  Eigen::Matrix<float,3,3> C_N2B;
  C_N2B(0,0) = 2.0f*powf(q(0,0),2.0f)-1.0f + 2.0f*powf(q(1,0),2.0f);
  C_N2B(1,1) = 2.0f*powf(q(0,0),2.0f)-1.0f + 2.0f*powf(q(2,0),2.0f);
  C_N2B(2,2) = 2.0f*powf(q(0,0),2.0f)-1.0f + 2.0f*powf(q(3,0),2.0f);

  C_N2B(0,1) = 2.0f*q(1,0)*q(2,0) + 2.0f*q(0,0)*q(3,0);
  C_N2B(0,2) = 2.0f*q(1,0)*q(3,0) - 2.0f*q(0,0)*q(2,0);

  C_N2B(1,0) = 2.0f*q(1,0)*q(2,0) - 2.0f*q(0,0)*q(3,0);
  C_N2B(1,2) = 2.0f*q(2,0)*q(3,0) + 2.0f*q(0,0)*q(1,0);

  C_N2B(2,0) = 2.0f*q(1,0)*q(3,0) + 2.0f*q(0,0)*q(2,0);
  C_N2B(2,1) = 2.0f*q(2,0)*q(3,0) - 2.0f*q(0,0)*q(1,0);
  return C_N2B;
}

// quaternion multiplication
Eigen::Matrix<float,4,1> uNavINS::qmult(Eigen::Matrix<float,4,1> p, Eigen::Matrix<float,4,1> q) {
  Eigen::Matrix<float,4,1> r;
  r(0,0) = p(0,0)*q(0,0) - (p(1,0)*q(1,0) + p(2,0)*q(2,0) + p(3,0)*q(3,0));
  r(1,0) = p(0,0)*q(1,0) + q(0,0)*p(1,0) + p(2,0)*q(3,0) - p(3,0)*q(2,0);
  r(2,0) = p(0,0)*q(2,0) + q(0,0)*p(2,0) + p(3,0)*q(1,0) - p(1,0)*q(3,0);
  r(3,0) = p(0,0)*q(3,0) + q(0,0)*p(3,0) + p(1,0)*q(2,0) - p(2,0)*q(1,0);
  return r;
}

// bound yaw angle between -180 and 180
float uNavINS::constrainAngle180(float dta) {
  if(dta >  M_PI) dta -= (M_PI*2.0f);
  if(dta < -M_PI) dta += (M_PI*2.0f);
  return dta;
}

// bound heading angle between 0 and 360
float uNavINS::constrainAngle360(float dta){
  dta = fmod(dta,2.0f*M_PI);
  if (dta < 0)
    dta += 2.0f*M_PI;
  return dta;
}

mjs513 said:
The problem we had before was related to the deltaT being incorrect. Once that was fixed it there should be no issues associated with the EKF unless your BNO055 or GPS data is corrupted.
Click to expand...
 
No good news.
nan's after 4 iterations of the program.

Im using the u-blox 8M (Radiolink SE100), the MPU9250 and all the messages required activated in u-center.

I really don't know what's it happening here. Once i connect the GPS, the angles start to alter their values randomly and then nan.

Im using exactly the same code you shared me, with the BNO055 and the MPU with same results.


mjs513 said:
Not sure what to tell you with your setup. Attached is the version of uNAVS INS I am using.

View attachment 27780

The other thing is are you using the sketch as I posted it or did you change it? It was working fine for me - no NaNs.
Click to expand...
 
@brtaylor and @mjs513: As I partially recall ...

@Juaxs:
In some early days working with this ( early version of this code? ) there was a NAN issue.
It was resolved and there was a good reason for it.
> Perhaps a search of this thread might point to that and suggest a solution if it if it relates to the behavior at hand.

May have been when the data was being packed and sent off with SPI_Master to do the math and prints on the Slave device? But there was a case where mysterious NAN's popped up in the process.
 
IM using the GPS module with Serial protocol. Thats why im using an earlier version.

defragster said:
@brtaylor and @mjs513: As I partially recall ...

@Juaxs:
In some early days working with this ( early version of this code? ) there was a NAN issue.
It was resolved and there was a good reason for it.
> Perhaps a search of this thread might point to that and suggest a solution if it if it relates to the behavior at hand.

May have been when the data was being packed and sent off with SPI_Master to do the math and prints on the Slave device? But there was a case where mysterious NAN's popped up in the process.
Click to expand...
 
Not been following the thread as far as version or other details - just saw the NAN - and there was an instance where the math came to that long ago ...
 
defragster said:
Just did a thread search on NAN and this post 3 yrs 11 months ago: pjrc.com/threads/48856-uNav-INS?p=175140&viewfull=1#post175140

Indicates the NAN problem then was the value of dt used in the calc - other posts before/after may offer added detail to suggest if that is the problem at hand there.
Click to expand...

Yep - see this: https://forum.pjrc.com/threads/48856-uNav-INS?p=300853&viewfull=1#post300853. The version of the unav I posted here: https://forum.pjrc.com/threads/48856-uNav-INS?p=302098&viewfull=1#post302098 has the fix incorporated. So theoretically if @Juaxs is using both the sketch and unav library I posted it should work. NaNs have to be coming from someplace else
 
At this point we're almost 60 posts deep into troubleshooting and haven't seen source code for debugging. Assuming that the outputs look reasonable for IMU, but not after receiving GNSS data, points to an issue in the measurement update and I'm guessing that some of the GNSS inputs are not as expected - either an issue with the units, reference frame, or sending the data before the GNSS has a good solution.

Can this EKF work without the lat/lon inputs (putting them in zero) and only use velocities on NED frame?
Click to expand...

No, the EKF would need to be modified. I recommend reading "Optimal State Estimation" by Dan Simon.
 
I posted my source code here and im using the program @mjs513 posted.

Code: 
/*
 *
      Note that by default the axis orientation of the BNO chip looks like
      the following (taken from section 3.4, page 24 of the datasheet).  Notice
      the dot in the corner that corresponds to the dot on the BNO chip:
                         | Z axis
                         |
                         |   / X axis
                     ____|__/____
        Y axis     / *   | /    /|
        _________ /______|/    //
                 /___________ //
                |____________|/
 */
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <EEPROM.h>
#include <uNavINS.h>
#include <ubx.h>

Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28); //3 lugar en EEPROM de la calibracion.

// a uNavINS object
uNavINS Filter;
bfs::Ubx gnss(&Serial1);
bool newGpsData;
unsigned long prevTOW;

// timers to measure performance
unsigned long tstart, tstop;
float acc_x, acc_y, acc_z;
float gyro_x, gyro_z, gyro_y;
float mag_x, mag_y, mag_z;

void setup() {
  Serial.begin(57600);
  if (!gnss.AutoBegin()) {
    Serial.println("Unable to establish communication and configure GNSS RX");
    while (1) {}
  }

  if (!bno.begin(bno.OPERATION_MODE_AMG))
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while (1);
  }
  //Set to axis remap p5
  //bno.setAxisRemap(bno.REMAP_CONFIG_P5);
  bno.setAxisSign(bno.REMAP_SIGN_P5);
  int eeAddress = 0;
  long bnoID;
  bool foundCalib = false;

  EEPROM.get(eeAddress, bnoID);

  adafruit_bno055_offsets_t calibrationData;
  sensor_t sensor;

  bno.getSensor(&sensor);
    if (bnoID != sensor.sensor_id)
    {
        Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
        delay(500);
    }
    else
    {
        Serial.println("\nFound Calibration for this sensor in EEPROM.");
        eeAddress += sizeof(long);
        EEPROM.get(eeAddress, calibrationData);

        //displaySensorOffsets(calibrationData);

        Serial.println("\n\nRestoring Calibration data to the BNO055...");
        bno.setSensorOffsets(calibrationData);

        Serial.println("\n\nCalibration data loaded into BNO055");
        foundCalib = true;
    }
}

void loop() {
  /* Get a new sensor event */
  sensors_event_t angVelocityData , magnetometerData, accelerometerData;

 if (gnss.Read() && gnss.num_sv() >= 3) {
    bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
    bno.getEvent(&magnetometerData, Adafruit_BNO055::VECTOR_MAGNETOMETER);
    bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);
  
    getIMU(&angVelocityData);
    getIMU(&magnetometerData);
    getIMU(&accelerometerData);

    tstart = micros();
  
    // update the filter
    Filter.update(gnss.gps_tow_s(),gnss.north_vel_mps(),gnss.east_vel_mps(),gnss.down_vel_mps(),gnss.lat_rad(),gnss.lon_rad(),gnss.alt_msl_m(), gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z,mag_x,mag_y,mag_z);
    Serial.print(Filter.getPitch_rad()*180.0f/PI);
    Serial.print(",");
    Serial.print(Filter.getRoll_rad()*180.0f/PI);
    Serial.print(",");
    Serial.println(Filter.getYaw_rad()*180.0f/PI);

    tstop = micros();

 }

}

void getIMU(sensors_event_t* event) {
      if (event->type == SENSOR_TYPE_ACCELEROMETER) {
        //Serial.print("Accl:");
        acc_x = event->acceleration.x;
        acc_y = event->acceleration.y;
        acc_z = event->acceleration.z;
        //Serial.print(acc_x, 4); Serial.print("\t");
        //Serial.print(acc_y, 4); Serial.print("\t");
        //Serial.println(acc_z, 4);
      } 
      else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
        //Serial.print("Mag:");
        mag_x = event->magnetic.x;
        mag_y = event->magnetic.y;
        mag_z = event->magnetic.z;
        //Serial.print(mag_x, 4); Serial.print("\t");
        //Serial.print(mag_y, 4); Serial.print("\t");
        //Serial.println(mag_z, 4);
      }
      else if (event->type == SENSOR_TYPE_GYROSCOPE) {
        //Serial.print("Gyro:");
        gyro_x = event->gyro.x;
        gyro_y = event->gyro.y;
        gyro_z = event->gyro.z;
        //Serial.print(gyro_x, 4); Serial.print("\t");
        //Serial.print(gyro_y, 4); Serial.print("\t");
        //Serial.println(gyro_z, 4);
      }
}

The GNSS units and frame reference are used exactly how is specter from the u-blox library.



brtaylor said:
At this point we're almost 60 posts deep into troubleshooting and haven't seen source code for debugging. Assuming that the outputs look reasonable for IMU, but not after receiving GNSS data, points to an issue in the measurement update and I'm guessing that some of the GNSS inputs are not as expected - either an issue with the units, reference frame, or sending the data before the GNSS has a good solution.



No, the EKF would need to be modified. I recommend reading "Optimal State Estimation" by Dan Simon.
Click to expand...
 
Hi
I really enjoyed the way you guys lead this project. Thank you for your sharings.

I am trying to make a stabilized structure, like a pedestal or gimbal. I have used an MPU9250, but need to increase the stability, specially solving the dead zone near 0 degree. so I decide to add a GPS module (and probably a barometer) to increase the AHRS precision. I know this might not be the best solution for my problem, so any other idea is welcomed.

BTW, here I have:
- mpu9250 module.
- UBLOX NEO m8n module
- bmp180 barometer

?1: could you please provide me an stable version of your project? I have read about 20 pages here and could not match a pair of sketch/lib. I will upload the last pair that I found in @mjs513 posts: View attachment MPU9250_FiltersGPS_V11a.zipView attachment uNavINS-master.zip . but still I cant compile. I probably should pay some time to remove the compilation errors, but It would be good to know these are the last version.
?2: could you provide the math formulation behind the EKF, so I can add barometer data to the solution?
 
RouhUAV said:
Hi
I really enjoyed the way you guys lead this project. Thank you for your sharings.

I am trying to make a stabilized structure, like a pedestal or gimbal. I have used an MPU9250, but need to increase the stability, specially solving the dead zone near 0 degree. so I decide to add a GPS module (and probably a barometer) to increase the AHRS precision. I know this might not be the best solution for my problem, so any other idea is welcomed.

BTW, here I have:
- mpu9250 module.
- UBLOX NEO m8n module
- bmp180 barometer

?1: could you please provide me an stable version of your project? I have read about 20 pages here and could not match a pair of sketch/lib. I will upload the last pair that I found in @mjs513 posts: View attachment 28828View attachment 28827 . but still I cant compile. I probably should pay some time to remove the compilation errors, but It would be good to know these are the last version.
?2: could you provide the math formulation behind the EKF, so I can add barometer data to the solution?
Click to expand...


So here I am, after a bunch of porting and reporting! it looks like I have come to match a sketch with a lib. I directly downloaded the libs from the github links bolderflight, instead of trusting the Arduino's lib manager. also I had to change a file name, Eigen -> eigen. and some other functions in the example.

But right now, I have problem with the uNavINS.h it self! it looks like its a C++ problem, I get a few warnings which says:

Code: 
C:\...\libraries\units-main\src/constants.h:44:20: warning: variable templates only available with -std=c++14 or -std=gnu++14
 static constexpr T G_MPS2 = static_cast<T>(9.80665);

C:\...\libraries\units-main\src/constants.h:37:20: warning: variable templates only available with -std=c++14 or -std=gnu++14
 static constexpr T BFS_PI =


and Also two errors :

Code: 
C:\...\libraries\ublox-main\src/ubx.h:161:3: error: 'elapsedMillis' does not name a type
   elapsedMillis t_ms_;



C:\...\libraries\uNavINS/uNavINS.h:145:5: error: 'elapsedMicros' does not name a type
     elapsedMicros _t;


Also the last which I have no idea what it means:

Code: 
cc1plus.exe: warning: unrecognized command line option '-Wno-frame-address'


It looks like my C++ compiler is outdated which does not support templates, but I am just using the last version of Arduino Desktop. whats wrong??:(:confused:


View attachment Mpu92GPSEkf_1.inoView attachment uNavINSedit.zip
 
Are you using Arduino with Teensyduino for compilation?

I've lost track of the uNavINS code on this forum, but the EKF was rolled into here:
https://github.com/bolderflight/navigation

ekf_15_state.h is the 15 state INS, which we use quite a bit and probably has some further code updates. I'll have to see if I have my notes on it, but it's basically follows the approach in "Optimal State Estimation" by Dan Simon with accel bias, gyro bias, NED velocity, position, and attitude states.
 
brtaylor said:
Are you using Arduino with Teensyduino for compilation?

I've lost track of the uNavINS code on this forum, but the EKF was rolled into here:
https://github.com/bolderflight/navigation

ekf_15_state.h is the 15 state INS, which we use quite a bit and probably has some further code updates. I'll have to see if I have my notes on it, but it's basically follows the approach in "Optimal State Estimation" by Dan Simon with accel bias, gyro bias, NED velocity, position, and attitude states.
Click to expand...


Dear Brian, Thank you for your kind response.

"Are you using Arduino with Teensyduino for compilation?"
I am afraid not, I planned to port it to other hardware platform, like ESP32 or STM32.

"EKF was rolled into here"
thanks.

And would you please share your experience, weather adding GPS can significantly improve AHRS outputs??
 
RouhUAV said:
"Are you using Arduino with Teensyduino for compilation?"
I am afraid not, I planned to port it to other hardware platform, like ESP32 or STM32.
Click to expand...

A lot of your errors are build-system related. The error below is indicating that you don't have C++14 or newer available with the compiler.

Code: 
C:\...\libraries\units-main\src/constants.h:44:20: warning: variable templates only available with -std=c++14 or -std=gnu++14
 static constexpr T G_MPS2 = static_cast<T>(9.80665);

This error is indicating that somehow it's not pulling in the Arduino core:

Code: 
C:\...\libraries\ublox-main\src/ubx.h:161:3: error: 'elapsedMillis' does not name a type
   elapsedMillis t_ms_;

This is a compilation option, but it's not being treated correctly.

Code: 
cc1plus.exe: warning: unrecognized command line option '-Wno-frame-address'

RouhUAV said:
And would you please share your experience, weather adding GPS can significantly improve AHRS outputs??
Click to expand...

It makes a huge difference in terms of velocity and position estimation. Without GPS, you're relying on integrating the accelerometers for velocity and then integrating again for position. Any error in the accelerometer measurement compounds quickly into large position errors given the double integration.

The main benefit for attitude estimation is for highly dynamic environments, like flight. In a traditional AHRS that only uses the IMU data, you are time integrating the gyro to estimate attitude and then using the accelerometer or magnetometer to arrest any integration errors. The issue is that in a dynamic environment, the acceleration can be caused by the vehicle motion in addition to gravity, so the accelerometer doesn't provide good data to arrest the gyro integration errors.

So, broadly speaking, if this is going on a vehicle, you'll want to use GPS data as your measurement update. If it's stationary on the ground, you're going to have better results using just the IMU data, since you won't have enough excitation of the GPS to get useful data out of it.
 
brtaylor is right. If your platform has any significant motion, you can't rely on the accelerometer to give you a second attitude reference (the first I assume is your magnetometer). Even with GPS aiding, and if you have a great dynamics model of your system, it's unlikely that you'll be able to retrieve an estimated gravity vector with enough accuracy or precision to use as a second reference using COTS MEMs sensors (that most of us can afford at least) - at least not good enough for a control system to use. I've tried this on many platforms with various levels of success. When I've gotten it to work, it was through artificially increasing terms in the covariance matrix to ignore the error buildup - but then I end up with a lot of drift in the attitude estimate. This is why it wasn't suitable for a control system without additional attitude measurements. It also depends on the system you are trying to control, its required bandwith and dynamics - smaller systems will need more sensitivity and bandwidth to control. I've ended up at this wall for hobbyist type hardware a few times. Make sure your control system, state estimator and overall system are matched to each other.

Attitude measurements, which are needed to correct the gyro stabilized dynamics model, are a tough topic - especially in the MEMS hobby world (read: us cheap bastards!). A properly calibrated magnetometer is usually the first attitude reference we all use since it's cheap and works on earth and in near-earth as long as you have a good magnetic model to use (see WMM). But additional attitude measurements to use if you are not motionless are a real challenge. Note, if you didn't know already, you'll need at least two (2) attitude references to estimate a 3-D attitude - but the more the better! RTK GPS can be used to provide an attitude reference (with 2+ receivers), and that equipment is getting more available and cheaper by the bay. But I'm not sure if it'll have the bandwidth for a pedestal balance type problem. Other options include camera object tracking (lot of CPU horsepower needed), time-of-flight type sensors, etc. Lots of ideas out there, but as usual implementation and bandwidth are a challenge.

This problem has certainly been solved in the past, but it'll use very expensive sensors, precision calibration of those sensors, and a system built to be controllable by the available hardware.

Good luck!
 
You must log in or register to reply here.
Back
Top