From 572c2280bd5d311145184fbad145e4d8b820f6eb Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sun, 1 May 2016 08:11:15 +1000
Subject: [PATCH] EKF: tidy up covariance prediction

---
 EKF/covariance.cpp | 33 ++++++++++++---------------------
 1 file changed, 12 insertions(+), 21 deletions(-)

diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index 4bc16b9efe..32aa91bfd1 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -173,30 +173,21 @@ void Ekf::predictCovariance()
 		wind_vel_sig = 0.0f;
 	}
 
+	// Construct the process noise variance diagonal for those states with a stationary process model
+	// These are kinematic states and their error growth is controlled separately by the IMU noise variances
 	for (unsigned i = 0; i <= 9; i++) {
 		process_noise[i] = 0.0f;
 	}
-
-	for (unsigned i = 10; i <= 12; i++) {
-		process_noise[i] = sq(d_ang_bias_sig);
-	}
-
-	for (unsigned i = 13; i <= 15; i++) {
-		process_noise[i] = sq(d_vel_bias_sig);
-	}
-
-	for (unsigned i = 16; i < 19; i++) {
-		process_noise[i] = sq(mag_I_sig);
-	}
-
-	for (unsigned i = 19; i < 22; i++) {
-		process_noise[i] = sq(mag_B_sig);
-	}
-
-	for (unsigned i = 22; i < 24; i++) {
-		process_noise[i] = sq(wind_vel_sig);
-	}
-
+	// delta angle bias states
+	process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig);
+	// delta_velocity bias states
+	process_noise[15] = process_noise[14] = process_noise[13] = sq(d_vel_bias_sig);
+	// earth frame magnetic field states
+	process_noise[18] = process_noise[17] = process_noise[16] = sq(mag_I_sig);
+	// body frame magnetic field states
+	process_noise[21] = process_noise[20] = process_noise[19] = sq(mag_B_sig);
+	// wind velocity states
+	process_noise[23] = process_noise[22] = sq(wind_vel_sig);
 
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities