GPS and IMU: State estimation improvements

sunray/LineTracker.cpp

@@ -291,7 +291,7 @@ void trackLine(bool runControl){
     }  
   } else {
     // no gps solution
-    if (REQUIRE_VALID_GPS){
+    if (REQUIRE_VALID_GPS && millis() > lastFixTime + INVALID_GPS_TIMEOUT * 1000.0){
       CONSOLE.println("WARN: no gps solution!");
       activeOp->onGpsNoSignal();
     }

sunray/StateEstimator.cpp

@@ -13,6 +13,8 @@
 #include "helper.h"
 #include "i2c.h"
 
+#include "src/math/vetor_datatype/quaternion_type.h"
+#include "src/math/vetor_datatype/vector_type.h"
 
 float stateX = 0;  // position-east (m)
 float stateY = 0;  // position-north (m)
@@ -40,6 +42,7 @@ float diffIMUWheelYawSpeedLP = 0;
 
 bool gpsJump = false;
 bool resetLastPos = true;
+unsigned long lastInvalidTime = 0;
 
 float lastIMUYaw = 0; 
 float lateralError = 0; // lateral error
@@ -211,7 +214,27 @@ void computeRobotState(){
   float distLeft = ((float)leftDelta) / ((float)motor.ticksPerCm);
   float distRight = ((float)rightDelta) / ((float)motor.ticksPerCm);  
   float distOdometry = (distLeft + distRight) / 2.0;
-  float deltaOdometry = -(distLeft - distRight) / motor.wheelBaseCm;    
+  float deltaOdometry = -(distLeft - distRight) / motor.wheelBaseCm;  
+
+  vec3_t rpy;
+  if ((imuDriver.imuFound) && (maps.useIMU)) {
+    // IMU available and should be used by planner
+    stateDelta = scalePI(stateDelta + stateDeltaIMU );
+    rpy = vec3_t(imuDriver.roll, imuDriver.pitch, stateDelta);
+  } else {
+    // odometry
+    stateDelta = scalePI(stateDelta + deltaOdometry);  
+    rpy = vec3_t(0.0, 0.0, stateDelta);
+  }
+
+  quat_t x; x.setRotation({1,0,0}, rpy.x, false);  
+  quat_t y; y.setRotation({0,1,0}, rpy.y, false);  
+  quat_t z; z.setRotation({0,0,1}, rpy.z, false);
+  quat_t rot = (z*y*x).norm();
+
+  vec3_t forward = rot.rotate({1,0,0}, GLOBAL_FRAME).norm(); 
+  vec3_t right = rot.rotate({0,-1,0}, GLOBAL_FRAME).norm();
+  vec3_t up = rot.rotate({0,0,1}, GLOBAL_FRAME).norm();
 
   float posN = 0;
   float posE = 0;
@@ -221,17 +244,48 @@ void computeRobotState(){
     posN = gps.relPosN;  
     posE = gps.relPosE;     
   }   
+
+  if (GPS_POSITION_OFFSET_ENABLED && imuDriver.imuFound)
+  {
+    vec3_t gpsOffset = forward * (GPS_POSITION_OFFSET_FORWARD / 100.0)
+                      + right * (GPS_POSITION_OFFSET_RIGHT / 100.0)
+                      + up * (GPS_POSITION_OFFSET_UP / 100.0);
+    posN += gpsOffset.y;
+    posE += gpsOffset.x;
+  }
+
 
-  if (fabs(motor.linearSpeedSet) < 0.001){       
+  if (fabs(motor.linearSpeedSet) < 0.001)  
     resetLastPos = true;
+
+  // detect fix jumps before heading fusion
+  if (gps.solutionAvail && gps.solution == SOL_FIXED
+  && sqrt( sq(posN-lastPosN)+sq(posE-lastPosE) ) > 0.2
+  && millis() > lastFixJumpTime + IGNORE_GPS_FIX_AFTER_JUMP * 1000.0
+  )
+  {
+    gps.solutionAvail = false;
+    lastFixJumpTime = millis();
+    lastPosN = posN;
+    lastPosE = posE;
   }
-
-  if ((gps.solutionAvail) 
-      && ((gps.solution == SOL_FIXED) || (gps.solution == SOL_FLOAT))  )
+  // set last invalid time
+  if (gps.solutionAvail && gps.solution == SOL_INVALID)
   {
-    gps.solutionAvail = false;        
+    gps.solutionAvail = false;
+    lastInvalidTime = millis();
+  }
+
+  if ((gps.solutionAvail) &&
+    ((gps.solution == SOL_FIXED 
+      && millis() > lastInvalidTime + IGNORE_GPS_FIX_AFTER_INVALID * 1000.0
+      && millis() > lastFixJumpTime + IGNORE_GPS_FIX_AFTER_JUMP * 1000.0)
+    || (gps.solution == SOL_FLOAT))  
+  )
+  {
+    gps.solutionAvail = false;
     stateGroundSpeed = 0.9 * stateGroundSpeed + 0.1 * abs(gps.groundSpeed);    
-    //CONSOLE.println(stateGroundSpeed);
+
     float distGPS = sqrt( sq(posN-lastPosN)+sq(posE-lastPosE) );
     if ((distGPS > 0.3) || (resetLastPos)){
       if (distGPS > 0.3) {
@@ -244,60 +298,62 @@ void computeRobotState(){
       lastPosN = posN;
       lastPosE = posE;
       lastPosDelta = stateDelta;
-    } else if (distGPS > 0.1) {       
+    }
+    else if (distGPS > 0.1) // gps-imu heading fusion
+    {       
       float diffLastPosDelta = distancePI(stateDelta, lastPosDelta);                 
-      if (fabs(diffLastPosDelta) /PI * 180.0 < 10){  // robot sensors indicate it is not turning
-        if ( (fabs(motor.linearSpeedSet) > 0) && (fabs(motor.angularSpeedSet) /PI *180.0 < 45) ) {  
-          stateDeltaGPS = scalePI(atan2(posN-lastPosN, posE-lastPosE));    
-          if (motor.linearSpeedSet < 0) stateDeltaGPS = scalePI(stateDeltaGPS + PI); // consider if driving reverse
-          //stateDeltaGPS = scalePI(2*PI-gps.heading+PI/2);
+      if (fabs(diffLastPosDelta) /PI * 180.0 < 10
+      && (fabs(motor.linearSpeedSet) > 0)
+      && (fabs(motor.angularSpeedSet) /PI *180.0 < 45) ) // make sure robot is not turning
+      {
+        stateDeltaGPS = scalePI(atan2(posN-lastPosN, posE-lastPosE));    
+        if (motor.linearSpeedSet < 0)
+          stateDeltaGPS = scalePI(stateDeltaGPS + PI); // consider if driving reverse
+
+        if (((gps.solution == SOL_FIXED) && (maps.useGPSfixForDeltaEstimation ))
+        || ((gps.solution == SOL_FLOAT) && false) ) // allows planner to use float solution?     
+        {    
           float diffDelta = distancePI(stateDelta, stateDeltaGPS);                 
-          if (    ((gps.solution == SOL_FIXED) && (maps.useGPSfixForDeltaEstimation ))
-              || ((gps.solution == SOL_FLOAT) && (maps.useGPSfloatForDeltaEstimation)) )
-          {   // allows planner to use float solution?         
-            if (fabs(diffDelta/PI*180) > 45){ // IMU-based heading too far away => use GPS heading
-              stateDelta = stateDeltaGPS;
-              stateDeltaIMU = 0;
-            } else {
-              // delta fusion (complementary filter, see above comment)
-              stateDeltaGPS = scalePIangles(stateDeltaGPS, stateDelta);
-              stateDelta = scalePI(fusionPI(0.9, stateDelta, stateDeltaGPS));               
-            }            
-          }
+          if (fabs(diffDelta/PI*180) > 45){ // IMU-based heading too far away => use GPS heading
+            stateDelta = stateDeltaGPS;
+            stateDeltaIMU = 0;
+          } else {
+            // delta fusion (complementary filter, see above comment)
+            stateDeltaGPS = scalePIangles(stateDeltaGPS, stateDelta);
+            stateDelta = fusionPI(0.9, stateDelta, stateDeltaGPS);               
+          }            
         }
       }
       lastPosN = posN;
       lastPosE = posE;
       lastPosDelta = stateDelta;
     } 
-    if (gps.solution == SOL_FIXED) {
-      // fix
+
+    // set last fix time
+    if (gps.solution == SOL_FIXED)
       lastFixTime = millis();
-      if (maps.useGPSfixForPosEstimation) {
-        stateX = posE;
-        stateY = posN;
-      }        
-    } else {
-      // float
-      if (maps.useGPSfloatForPosEstimation){ // allows planner to use float solution?
-        stateX = posE;
-        stateY = posN;              
-      }
+
+    // update state for fix and float
+    if (gps.solution == SOL_FIXED && maps.useGPSfixForPosEstimation){ // fix
+      stateX = posE;
+      stateY = posN;
     }
-  } 
+    else if (maps.useGPSfloatForPosEstimation){ // allows planner to use float solution?
+      vec3_t pos = (vec3_t(stateX, stateY, 0.0) + forward * (distOdometry/100.0)) * 0.999
+                  + vec3_t(posE, posN, 0.0) * 0.001;
+      stateX = pos.x;
+      stateY = pos.y;           
+    }
+  }
+  else // no GPS data available, use odometry
+  {
+    vec3_t pos = forward * (distOdometry/100.0);
+    stateX += pos.x;
+    stateY += pos.y;
+  }
 
-  // odometry
-  stateX += distOdometry/100.0 * cos(stateDelta);
-  stateY += distOdometry/100.0 * sin(stateDelta);        
   if (stateOp == OP_MOW) statMowDistanceTraveled += distOdometry/100.0;
 
-  if ((imuDriver.imuFound) && (maps.useIMU)) {
-    // IMU available and should be used by planner
-    stateDelta = scalePI(stateDelta + stateDeltaIMU );          
-  } else {
-    // odometry
-    stateDelta = scalePI(stateDelta + deltaOdometry);  
-  }
   if (imuDriver.imuFound){
     stateDeltaSpeedIMU = 0.99 * stateDeltaSpeedIMU + 0.01 * stateDeltaIMU / 0.02; // IMU yaw rotation speed (20ms timestep)
   }
@@ -321,5 +377,3 @@ void computeRobotState(){
     //CONSOLE.println(stateDeltaSpeedWheels/PI*180.0);
   }
 }
-
-

sunray/StateEstimator.h

@@ -30,6 +30,8 @@ extern float diffIMUWheelYawSpeed;
 extern float diffIMUWheelYawSpeedLP;
 
 extern bool gpsJump;
+extern unsigned long lastInvalidTime;
+extern unsigned long lastFixJumpTime;
 
 extern bool imuIsCalibrating;
 extern unsigned long imuDataTimeout;

sunray/config_example.h

@@ -345,6 +345,16 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 #define CPG_CONFIG_FILTER_NCNOTHRS 10   // C/N0 Threshold #SVs: 10 (robust), 6 (less robust)
 #define CPG_CONFIG_FILTER_CNOTHRS  30   // 30 dbHz (robust), 13 dbHz (less robust)
 
+#define GPS_POSITION_OFFSET_ENABLED false // enable gps antenna position correction offset?
+#define GPS_POSITION_OFFSET_FORWARD 0.0 // offset to add fordwards (cm)
+#define GPS_POSITION_OFFSET_RIGHT 0.0 // offset to add to the right (cm)
+#define GPS_POSITION_OFFSET_UP 0.0 // offse to add upwards (cm), set to ground level for tilt compensation (-24 cm for standard ardumower) 
+
+#define INVALID_GPS_TIMEOUT 0 // time in seconds to navigate with invalid GPS
+
+#define IGNORE_GPS_FIX_AFTER_INVALID 0.0 // time in seconds to ignore GPS fix after recovering from invalid solution
+#define IGNORE_GPS_FIX_AFTER_JUMP 0.0 // time in seconds to ignore GPS fix after fix jump detected
+
 
 // ------ obstacle detection and avoidance  -------------------------
 
@@ -387,6 +397,7 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 
 #define UNDOCK_IGNORE_GPS_DISTANCE 2 // set distance (m) from dock to ignore gps while undocking
 
+
 // ---- path tracking -----------------------------------
 
 // below this robot-to-target distance (m) a target is considered as reached