EKF: Enable tuning for IMU switch on bias errors

EKF/common.h

@@ -197,6 +197,10 @@ struct parameters {
 	float terrain_p_noise;		// process noise for terrain offset (m/sec)
 	float terrain_gradient;		// gradient of terrain used to estimate process noise due to changing position (m/m)
 
+	// initial switch on bias uncertainty
+	float switch_on_gyro_bias;	// 1-sigma gyro bias uncertainty at switch on (rad/sec)
+	float switch_on_accel_bias;	// 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
+
 	// position and velocity fusion
 	float gps_vel_noise;		// observation noise for gps velocity fusion (m/sec)
 	float gps_pos_noise;		// observation noise for gps position fusion (m)
@@ -288,6 +292,10 @@ struct parameters {
 		terrain_p_noise = 5.0f;
 		terrain_gradient = 0.5f;
 
+		// initial switch on bias uncertainty
+		switch_on_gyro_bias = 0.1f;
+		switch_on_accel_bias = 0.2f;
+
 		// position and velocity fusion
 		gps_vel_noise = 5.0e-1f;
 		gps_pos_noise = 0.5f;

EKF/covariance.cpp

@@ -80,12 +80,12 @@ void Ekf::initialiseCovariance()
 	}
 
 	// gyro bias
-	P[10][10] = sq(0.1f * dt);
+	P[10][10] = sq(_params.switch_on_gyro_bias * dt);
 	P[11][11] = P[10][10];
 	P[12][12] = P[10][10];
 
 	// accel bias
-	P[13][13] = sq(0.2f * dt);
+	P[13][13] = sq(_params.switch_on_accel_bias * dt);
 	P[14][14] = P[13][13];
 	P[15][15] = P[13][13];