From e06162fa3a5b8781de562fc191152a194116a491 Mon Sep 17 00:00:00 2001
From: peabody124 <peabody124>
Date: Mon, 6 Sep 2010 07:54:19 +0000
Subject: [PATCH] Flight/AHRS: Reorganized the code a little to make sure
 indoor used by default unless GPS updates come  in.  Also change the velocity
 variance in indoor mode to lower to make it get a better attitude estimate
 (primarily faster convergence - probably will tweak this in the future
 dynamically).  Finally decreased barometer variance - in scope it's actually
 quite reasonable, normally within +/- .5 m.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1546 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/AHRS/ahrs.c   | 45 ++++++++++++++++++++------------------------
 flight/AHRS/insgps.c |  2 +-
 2 files changed, 21 insertions(+), 26 deletions(-)

diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 529efcd67..8d0dbefa7 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -225,6 +225,7 @@ int main()
 {
     float gyro[3], accel[3], mag[3];
     float vel[3] = {0,0,0};
+	gps_data.quality = -1;
     
     ahrs_algorithm = INSGPS_Algo;
     
@@ -356,35 +357,29 @@ int main()
             
             INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
             
-            if ( gps_data.updated ) 
+            if ( gps_data.updated && gps_data.quality == 1) 
             {
-                if(gps_data.quality == 1) 
-                {
-                    // Compute velocity from Heading and groundspeed
-                    vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);        
-                    vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
-                    
-                    // Completely unprincipled way to make the position variance
-                    // increase as data quality decreases but keep it bounded
-                    // Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
-                    INSSetPosVelVar(0.004);
-                    FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);        
-                    gps_data.updated = false;
-                } else if(gps_data.quality == 0) {
-                    // Not indoors but lost lock. 
-                    // TODO: Want to clamp coviance matrix to initial conditions on diagonal
-                    MagCorrection(mag);
-                } else if(gps_data.quality == -1) {
-                    // Indoors, update with zero position and velocity and high covariance
-                    INSSetPosVelVar(10);
-                    vel[0] = 0;
-                    vel[1] = 0;
-                    vel[2] = 0;
-                    VelBaroCorrection(vel,altitude_data.altitude);                            
-                } 
+				// Compute velocity from Heading and groundspeed
+				vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);        
+				vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
+				
+				// Completely unprincipled way to make the position variance
+				// increase as data quality decreases but keep it bounded
+				// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
+				INSSetPosVelVar(0.004);
+				FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);        
+				gps_data.updated = false;
             }
             else if(gps_data.quality != -1)
                 MagCorrection(mag);  // only trust mags if outdoors
+			else {
+				// Indoors, update with zero position and velocity and high covariance
+				INSSetPosVelVar(0.1);
+				vel[0] = 0;
+				vel[1] = 0;
+				vel[2] = 0;
+				VelBaroCorrection(vel,altitude_data.altitude);                            				
+			}
             
             attitude_data.quaternion.q1 = Nav.q[0];
             attitude_data.quaternion.q2 = Nav.q[1];
diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c
index 866dcb7ae..461dd3abb 100644
--- a/flight/AHRS/insgps.c
+++ b/flight/AHRS/insgps.c
@@ -82,7 +82,7 @@ void INSGPSInit()   //pretty much just a place holder for now
 	R[3]=R[4]=0.004;       // High freq GPS horizontal velocity noise variance (m/s)^2
 	R[5]=100;              // High freq GPS vertical velocity noise variance (m/s)^2
 	R[6]=R[7]=R[8]=0.005;  // magnetometer unit vector noise variance
-	R[9]=1;                // High freq altimeter noise variance (m^2)
+	R[9]=.05;              // High freq altimeter noise variance (m^2)
 }
 
 void INSSetPosVelVar(float PosVar)