From f2ca1aad083aa793b56ff963c8c84580cfec0045 Mon Sep 17 00:00:00 2001 From: Brian Taylor Date: Wed, 3 Jan 2018 10:56:10 -0800 Subject: [PATCH] Initial commit --- README.md | 191 +++++++- .../CalibrateMPU9250/CalibrateMPU9250.ino | 103 +++++ .../uNavINS_MPU9250/uNavINS_MPU9250.ino | 91 ++++ keywords.txt | 19 + uNavINS.cpp | 433 ++++++++++++++++++ uNavINS.h | 196 ++++++++ 6 files changed, 1032 insertions(+), 1 deletion(-) create mode 100644 examples/uNavINS-with-MPU9250/CalibrateMPU9250/CalibrateMPU9250.ino create mode 100644 examples/uNavINS-with-MPU9250/uNavINS_MPU9250/uNavINS_MPU9250.ino create mode 100644 keywords.txt create mode 100644 uNavINS.cpp create mode 100644 uNavINS.h diff --git a/README.md b/README.md index 41f7b41..2404afa 100644 --- a/README.md +++ b/README.md @@ -1 +1,190 @@ -# uNavINS \ No newline at end of file +# uNavINS +uNav Inertial Navigation System 15 State EKF Arduino Library. + +# Description +The uNav Inertial Navigation System (INS) is a 15 state Extended Kalman Filter (EKF) to estimate the following from IMU and GPS data: +* Attitude +* Latitude, longitude, and altitude (LLA) +* North, east, down (NED) inertial velocity +* Ground track + +The 15 states comprise the inertial position, inertial velocity, a quaternion, accelerometer biases, and gyro biases. This algorithm was developed by Adhika Lie at the [University of Minnesota UAS Research Labs](http://www.uav.aem.umn.edu), where it has been used since 2006 as a baseline navigation algorithm to gauge the performance of other algorithms in simulation studies and flight tests. uNav INS provides excellent estimates of attitude, inertial position, and inertial velocity once it has converged on a solution. + +XXX - how to tell that solution has converged? How to warm up filter? + +# Usage + +## Installation +This library requires [Eigen](https://github.com/bolderflight/Eigen) to compile. First download or clone [Eigen](https://github.com/bolderflight/Eigen) into your Arduino/libraries folder, then download or clone this library into your Arduino/libraries folder. Additionally, this library requires IMU measurements and GPS measurements. For the included examples, an [MPU-9250 IMU](https://github.com/bolderflight/MPU9250) is used with a [uBlox GPS](https://github.com/bolderflight/UBLOX), and their libraries will need to be installed as well. + +XXX - Finally, because this library is using accelerometers and magnetometers as a measurement update, the IMU used should be well calibrated. + +## Function Description + +### Object Declaration +This library uses the default constructor. The following is an example of declaring a uNavINS object called *Filter*. + +```C++ +uNavINS Filter; +``` + +### Data Collection Functions + +**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)** updates the filter with new IMU and GPS measurements, time updates propogate the state and measurement updates are made; the attitude and inertial position and velocity estimates of the vehicle are updated. Inputs are: + +* unsigned long TOW: GPS time of week. This is used to trigger a measurement update when the GPS data is new (i.e. the TOW has changed). +* double vn: GPS velocity in the North direction, units are m/s. +* double ve: GPS velocity in the East direction, units are m/s. +* double vd: GPS velocity in the down direction, units are m/s. +* double lat: GPS measured latitude, units are rad. +* double lon: GPS measured longitude, units are rad. +* double alt: GPS measured altitude, units are m. +* float p: gyro measurement in the x direction, units are rad/s. +* float q: gyro measurement in the y direction, units are rad/s. +* float r: gyro measurement in the z direction, units are rad/s. +* float ax: accelerometer measurement in the x direction, units are m/s/s. +* float ay: accelerometer measurement in the y direction, units are m/s/s. +* float az: accelerometer measurement in the z direction, units are m/s/s. +* float hx: magnetometer measurement in the x direction, units need to be consistant across all magnetometer measurements used. +* float hy: magnetometer measurement in the y direction, units need to be consistant across all magnetometer measurements used. +* float hz: magnetometer measurement in the z direction, units need to be consistant across all magnetometer measurements used. + +Please note that IMU measurements need to be given in the [defined axis system](#axis-system). + +```C++ +// read the sensor +Imu.readSensor(); +// update the filter +Filter.update(uBloxData.iTOW,uBloxData.velN,uBloxData.velE,uBloxData.velD,uBloxData.lat*PI/180.0f,uBloxData.lon*PI/180.0f,uBloxData.hMSL,Imu.getGyroY_rads(),-1*Imu.getGyroX_rads(),Imu.getGyroZ_rads(),Imu.getAccelY_mss(),-1*Imu.getAccelX_mss(),Imu.getAccelZ_mss(),Imu.getMagX_uT(),Imu.getMagY_uT(),Imu.getMagZ_uT()); +``` + +**float getRoll_rad()** returns the roll angle in units of rad. + +```C++ +float roll; +roll = Filter.getRoll_rad(); +``` + +**float getPitch_rad()** returns the pitch angle in units of rad. + +```C++ +float pitch; +pitch = Filter.getPitch_rad(); +``` + +**float getYaw_rad()** returns the yaw angle in units of rad. + +```C++ +float yaw; +yaw = Filter.getYaw_rad(); +``` + +**float getHeading_rad()** returns the heading angle in units of rad. + +```C++ +float heading; +heading = Filter.getHeading_rad(); +``` + +**double getLatitude_rad** returns the INS estimated latitude in units of rad. + +```C++ +double latitude; +latitude = Filter.getLatitude_rad(); +``` + +**double getLongitude_rad** returns the INS estimated longitude in units of rad. + +```C++ +double longitude; +longitude = Filter.getLongitude_rad(); +``` + +**double getAltitude_m** returns the INS estimated altitude in units of m. + +```C++ +double alt; +alt = Filter.getAltitude_m(); +``` + +**double getVelNorth_ms** returns the INS estimated inertial velocity in the North direction in units of m/s. + +```C++ +double vn; +vn = Filter.getVelNorth_ms(); +``` + +**double getVelEast_ms** returns the INS estimated inertial velocity in the East direction in units of m/s. + +```C++ +double ve; +ve = Filter.getVelEast_ms(); +``` + +**double getVelDown_ms** returns the INS estimated inertial velocity in the down direction in units of m/s. + +```C++ +double vd; +vd = Filter.getVelDown_ms(); +``` + +**float getGroundTrack_rad** returns the INS estimated inertial ground track in units of rad. + +```C++ +float track; +track = Filter.getGroundTrack_rad(); +``` + +**float getGyroBiasX_rads** returns the current gyro bias in the x direction in units of rad/s. + +```C++ +float gxb; +gxb = Filter.getGyroBiasX_rads(); +``` + +**float getGyroBiasY_rads** returns the current gyro bias in the y direction in units of rad/s. + +```C++ +float gyb; +gyb = Filter.getGyroBiasY_rads(); +``` + +**float getGyroBiasZ_rads** returns the current gyro bias in the z direction in units of rad/s. + +```C++ +float gzb; +gzb = Filter.getGyroBiasZ_rads(); +``` + +**float getAccelBiasX_mss** returns the current accelerometer bias in the x direction in units of m/s/s. + +```C++ +float axb; +axb = Filter.getAccelBiasX_mss(); +``` + +**float getAccelBiasY_mss** returns the current accelerometer bias in the Y direction in units of m/s/s. + +```C++ +float ayb; +ayb = Filter.getAccelBiasY_mss(); +``` + +**float getAccelBiasZ_mss** returns the current accelerometer bias in the Z direction in units of m/s/s. + +```C++ +float azb; +azb = Filter.getAccelBiasZ_mss(); +``` + +## Axis System +This library expects IMU data to be input in a defined axis system, which is shown below. It is a right handed coordinate system with x-axis pointed forward, the y-axis to the right, and the z-axis positive down, common in aircraft dynamics. Pitch is defined as a rotation angle around the y-axis with level as zero and roll is defined as a rotation angle around the x-axis with level as zero. Yaw is defined as a rotation angle around the z-axis with zero defined as the starting orientation. Heading is defined as a rotation angle around the z-axis with zero defined as magnetic north. + +Common Axis System + +## Example List +* **uNavINS-with-MPU9250**: demonstrates using this filter with an MPU-9250 IMU. *CalibrateMPU9250.ino* is used to calibrate the MPU-9250 IMU and store the calibration coefficients in EEPROM. *uNavINS_MPU9250.ino* uses the MPU-9250 IMU and uBlox GPS as measurement inputs to the uNav INS filter, which is run at a rate of 100 Hz. + +# References + +1. FILL diff --git a/examples/uNavINS-with-MPU9250/CalibrateMPU9250/CalibrateMPU9250.ino b/examples/uNavINS-with-MPU9250/CalibrateMPU9250/CalibrateMPU9250.ino new file mode 100644 index 0000000..5028788 --- /dev/null +++ b/examples/uNavINS-with-MPU9250/CalibrateMPU9250/CalibrateMPU9250.ino @@ -0,0 +1,103 @@ +/* +CalibrateMPU9250.ino +Brian R Taylor +brian.taylor@bolderflight.com + +Copyright (c) 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 "MPU9250.h" +#include "EEPROM.h" + +// an MPU-9250 object on SPI bus 0 with chip select 10 +MPU9250 Imu(SPI,10); +int status; + +// EEPROM buffer and variable to save accel and mag bias +// and scale factors +uint8_t EepromBuffer[48]; +float value; + +void setup() { + // serial to display instructions + Serial.begin(115200); + while(!Serial) {} + // start communication with IMU + status = Imu.begin(); + if (status < 0) { + Serial.println("IMU initialization unsuccessful"); + Serial.println("Check IMU wiring or try cycling power"); + Serial.print("Status: "); + Serial.println(status); + while(1) {} + } + // calibrating accelerometer + Serial.println("Starting Accelerometer Calibration"); + Imu.calibrateAccel(); + Serial.println("Switch"); + delay(5000); + Imu.calibrateAccel(); + Serial.println("Switch"); + delay(5000); + Imu.calibrateAccel(); + Serial.println("Switch"); + delay(5000); + Imu.calibrateAccel(); + Serial.println("Switch"); + delay(5000); + Imu.calibrateAccel(); + Serial.println("Switch"); + delay(5000); + Imu.calibrateAccel(); + Serial.println("Done"); + Serial.println("Starting Magnetometer Calibration"); + delay(5000); + // calibrating magnetometer + Imu.calibrateMag(); + // saving to EEPROM + value = Imu.getAccelBiasX_mss(); + memcpy(EepromBuffer,&value,4); + value = Imu.getAccelScaleFactorX(); + memcpy(EepromBuffer+4,&value,4); + value = Imu.getAccelBiasY_mss(); + memcpy(EepromBuffer+8,&value,4); + value = Imu.getAccelScaleFactorY(); + memcpy(EepromBuffer+12,&value,4); + value = Imu.getAccelBiasZ_mss(); + memcpy(EepromBuffer+16,&value,4); + value = Imu.getAccelScaleFactorZ(); + memcpy(EepromBuffer+20,&value,4); + value = Imu.getMagBiasX_uT(); + memcpy(EepromBuffer+24,&value,4); + value = Imu.getMagScaleFactorX(); + memcpy(EepromBuffer+28,&value,4); + value = Imu.getMagBiasY_uT(); + memcpy(EepromBuffer+32,&value,4); + value = Imu.getMagScaleFactorY(); + memcpy(EepromBuffer+36,&value,4); + value = Imu.getMagBiasZ_uT(); + memcpy(EepromBuffer+40,&value,4); + value = Imu.getMagScaleFactorZ(); + memcpy(EepromBuffer+44,&value,4); + for (size_t i=0; i < sizeof(EepromBuffer); i++) { + EEPROM.write(i,EepromBuffer[i]); + } + Serial.println("Done"); +} + +void loop() {} diff --git a/examples/uNavINS-with-MPU9250/uNavINS_MPU9250/uNavINS_MPU9250.ino b/examples/uNavINS-with-MPU9250/uNavINS_MPU9250/uNavINS_MPU9250.ino new file mode 100644 index 0000000..3453ada --- /dev/null +++ b/examples/uNavINS-with-MPU9250/uNavINS_MPU9250/uNavINS_MPU9250.ino @@ -0,0 +1,91 @@ +/* +uNavINS_MPU9250.ino +Brian R Taylor +brian.taylor@bolderflight.com + +Copyright (c) 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" +#include "MPU9250.h" +#include "UBLOX.h" + +// an MPU-9250 object on SPI bus 0 with chip select 24 +MPU9250 Imu(SPI,24); +int status; +// a flag for when the MPU-9250 has new data +volatile int newData; +// a uNavINS object +uNavINS Filter; +UBLOX gps(4); +bool newGpsData; +unsigned long prevTOW; +gpsData uBloxData; +// timers to measure performance +unsigned long tstart, tstop; + +void setup() { + // serial to display data + Serial.begin(115200); + while(!Serial) {} + + gps.begin(115200); + + // start communication with IMU + status = Imu.begin(); + if (status < 0) { + Serial.println("IMU initialization unsuccessful"); + Serial.println("Check IMU wiring or try cycling power"); + Serial.print("Status: "); + Serial.println(status); + while(1) {} + } + // setting SRD to 9 for a 100 Hz update rate + Imu.setSrd(9); + // enabling the data ready interrupt + Imu.enableDataReadyInterrupt(); + // attaching the interrupt to microcontroller pin 1 + pinMode(27,INPUT); + attachInterrupt(27,runFilter,RISING); + Serial.println("Starting..."); +} + +void loop() { + gps.read(&uBloxData); + if (uBloxData.numSV > 5) { + if (newData == 1) { + newData = 0; + tstart = micros(); + // read the sensor + Imu.readSensor(); + // update the filter + Filter.update(uBloxData.iTOW,uBloxData.velN,uBloxData.velE,uBloxData.velD,uBloxData.lat*PI/180.0f,uBloxData.lon*PI/180.0f,uBloxData.hMSL,Imu.getGyroY_rads(),-1*Imu.getGyroX_rads(),Imu.getGyroZ_rads(),Imu.getAccelY_mss(),-1*Imu.getAccelX_mss(),Imu.getAccelZ_mss(),Imu.getMagX_uT(),Imu.getMagY_uT(),Imu.getMagZ_uT()); + Serial.print(Filter.getPitch_rad()*180.0f/PI); + Serial.print("\t"); + Serial.print(Filter.getRoll_rad()*180.0f/PI); + Serial.print("\t"); + Serial.print(Filter.getYaw_rad()*180.0f/PI); + Serial.print("\n"); + tstop = micros(); + } + } +} + +void runFilter() { + newData = 1; +} diff --git a/keywords.txt b/keywords.txt new file mode 100644 index 0000000..15f5d3e --- /dev/null +++ b/keywords.txt @@ -0,0 +1,19 @@ +uNavINS KEYWORD1 +update KEYWORD2 +getPitch_rad KEYWORD2 +getRoll_rad KEYWORD2 +getYaw_rad KEYWORD2 +getHeading_rad KEYWORD2 +getLatitude_rad KEYWORD2 +getLongitude_rad KEYWORD2 +getAltitude_m KEYWORD2 +getVelNorth_ms KEYWORD2 +getVelEast_ms KEYWORD2 +getVelDown_ms KEYWORD2 +getGroundTrack_rad KEYWORD2 +getGyroBiasX_rads KEYWORD2 +getGyroBiasY_rads KEYWORD2 +getGyroBiasZ_rads KEYWORD2 +getAccelBiasX_mss KEYWORD2 +getAccelBiasY_mss KEYWORD2 +getAccelBiasZ_mss KEYWORD2 \ No newline at end of file diff --git a/uNavINS.cpp b/uNavINS.cpp new file mode 100644 index 0000000..610d54d --- /dev/null +++ b/uNavINS.cpp @@ -0,0 +1,433 @@ +/* +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 "Arduino.h" +#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) { + // set the time + tprev = (float) micros()/1000000.0f; + // 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 = PI/2.0f - atanf(Bxc/-Byc); + } else { + psi= 3.0f*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(5,5,0,0) = Eigen::Matrix::Identity(); + // // ... Rw + // Rw.block(3,3,0,0) = powf(SIG_W_A,2.0f)*Eigen::Matrix::Identity(); + // Rw.block(3,3,3,3) = powf(SIG_W_G,2.0f)*Eigen::Matrix::Identity(); + // Rw.block(3,3,6,6) = 2.0f*powf(SIG_A_D,2.0f)/TAU_A*Eigen::Matrix::Identity(); + // Rw.block(3,3,9,9) = 2.0f*powf(SIG_G_D,2.0f)/TAU_G*Eigen::Matrix::Identity(); + // // ... P + // P.block(3,3,0,0) = powf(P_P_INIT,2.0f)*Eigen::Matrix::Identity(); + // P.block(3,3,3,3) = powf(P_V_INIT,2.0f)*Eigen::Matrix::Identity(); + // P.block(2,2,6,6) = powf(P_A_INIT,2.0f)*Eigen::Matrix::Identity(); + // P(8,8) = powf(P_HDG_INIT,2.0f); + // P.block(3,3,9,9) = powf(P_AB_INIT,2.0f)*Eigen::Matrix::Identity(); + // P.block(3,3,12,12) = powf(P_GB_INIT,2.0f)*Eigen::Matrix::Identity(); + // // ... R + // R.block(2,2,0,0) = powf(SIG_GPS_P_NE,2.0f)*Eigen::Matrix::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::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; + // initialized flag + initialized = true; + } else { + // get the change in time + tnow = (float) micros()/1000000.0f; + dt = tnow - tprev; + tprev = tnow; + 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(3,3,0,3) = Eigen::Matrix::Identity(); + // ... gs2pos + Fs(5,2) = -2.0f*G/EARTH_RADIUS; + // ... gs2att + Fs.block(3,3,3,6) = -2.0f*C_B2N*sk(f_b); + // ... gs2acc + Fs.block(3,3,3,9) = -C_B2N; + // ... att2att + Fs.block(3,3,6,6) = -sk(om_ib); + // ... att2gyr + Fs.block(3,3,6,12) = -0.5f*Eigen::Matrix::Identity(); + // ... Accel Markov Bias + Fs.block(3,3,9,9) = -1.0f/TAU_A*Eigen::Matrix::Identity(); + Fs.block(3,3,12,12) = -1.0f/TAU_G*Eigen::Matrix::Identity(); + // State Transition Matrix + PHI = Eigen::Matrix::Identity()+Fs*dt; + // Process Noise + Gs.setZero(); + Gs.block(3,3,3,0) = -C_B2N; + Gs.block(3,3,6,3) = -0.5f*Eigen::Matrix::Identity(); + Gs.block(6,6,9,6) = Eigen::Matrix::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::Identity()-K*H)*P*(Eigen::Matrix::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 uNavINS::sk(Eigen::Matrix w) { + Eigen::Matrix 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 uNavINS::llarate(Eigen::Matrix V,Eigen::Matrix lla) { + double Rew, Rns, denom; + Eigen::Matrix 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 uNavINS::lla2ecef(Eigen::Matrix lla) { + double Rew, denom; + Eigen::Matrix 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 uNavINS::ecef2ned(Eigen::Matrix ecef,Eigen::Matrix pos_ref) { + Eigen::Matrix 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 uNavINS::quat2dcm(Eigen::Matrix q) { + Eigen::Matrix 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 uNavINS::qmult(Eigen::Matrix p, Eigen::Matrix q) { + Eigen::Matrix 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 > PI) dta -= (PI*2.0f); + if(dta < -PI) dta += (PI*2.0f); + return dta; +} + +// bound heading angle between 0 and 360 +float uNavINS::constrainAngle360(float dta){ + dta = fmod(dta,2.0f*PI); + if (dta < 0) + dta += 2.0f*PI; + return dta; +} diff --git a/uNavINS.h b/uNavINS.h new file mode 100644 index 0000000..637ac44 --- /dev/null +++ b/uNavINS.h @@ -0,0 +1,196 @@ +/* +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 + +#include "Arduino.h" +#include "Eigen.h" +#include + +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 + float tnow, tprev, 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 Fs = Eigen::Matrix::Zero(); + // State transition matrix + Eigen::Matrix PHI = Eigen::Matrix::Zero(); + // Covariance matrix + Eigen::Matrix P = Eigen::Matrix::Zero(); + // For process noise transformation + Eigen::Matrix Gs = Eigen::Matrix::Zero(); + Eigen::Matrix Rw = Eigen::Matrix::Zero(); + // Process noise matrix + Eigen::Matrix Q = Eigen::Matrix::Zero(); + // Gravity model + Eigen::Matrix grav = Eigen::Matrix::Zero(); + // Rotation rate + Eigen::Matrix om_ib = Eigen::Matrix::Zero(); + // Specific force + Eigen::Matrix f_b = Eigen::Matrix::Zero(); + // DCM + Eigen::Matrix C_N2B = Eigen::Matrix::Zero(); + // DCM transpose + Eigen::Matrix C_B2N = Eigen::Matrix::Zero(); + // Temporary to get dxdt + Eigen::Matrix dx = Eigen::Matrix::Zero(); + Eigen::Matrix dxd = Eigen::Matrix::Zero(); + // NED velocity INS + Eigen::Matrix V_ins = Eigen::Matrix::Zero(); + // LLA INS + Eigen::Matrix lla_ins = Eigen::Matrix::Zero(); + // NED velocity GPS + Eigen::Matrix V_gps = Eigen::Matrix::Zero(); + // LLA GPS + Eigen::Matrix lla_gps = Eigen::Matrix::Zero(); + // Position ECEF INS + Eigen::Matrix pos_ecef_ins = Eigen::Matrix::Zero(); + // Position NED INS + Eigen::Matrix pos_ned_ins = Eigen::Matrix::Zero(); + // Position ECEF GPS + Eigen::Matrix pos_ecef_gps = Eigen::Matrix::Zero(); + // Position NED GPS + Eigen::Matrix pos_ned_gps = Eigen::Matrix::Zero(); + // Quat + Eigen::Matrix quat = Eigen::Matrix::Zero(); + // dquat + Eigen::Matrix dq = Eigen::Matrix::Zero(); + // difference between GPS and INS + Eigen::Matrix y = Eigen::Matrix::Zero(); + // GPS measurement noise + Eigen::Matrix R = Eigen::Matrix::Zero(); + Eigen::Matrix x = Eigen::Matrix::Zero(); + // Kalman Gain + Eigen::Matrix K = Eigen::Matrix::Zero(); + Eigen::Matrix H = Eigen::Matrix::Zero(); + // skew symmetric + Eigen::Matrix sk(Eigen::Matrix w); + // lla rate + Eigen::Matrix llarate(Eigen::Matrix V,Eigen::Matrix lla); + // lla to ecef + Eigen::Matrix lla2ecef(Eigen::Matrix lla); + // ecef to ned + Eigen::Matrix ecef2ned(Eigen::Matrix ecef,Eigen::Matrix pos_ref); + // quaternion to dcm + Eigen::Matrix quat2dcm(Eigen::Matrix q); + // quaternion multiplication + Eigen::Matrix qmult(Eigen::Matrix p, Eigen::Matrix q); + // maps angle to +/- 180 + float constrainAngle180(float dta); + // maps angle to 0-360 + float constrainAngle360(float dta); +}; + +#endif