Merge pull request #183 from CarlOlsson/cont_wind_estimation

Fix wind state  and wind variance initialization

EKF/airspeed_fusion.cpp

@@ -150,18 +150,25 @@ void Ekf::fuseAirspeed()
 		// Airspeed measurement sample has passed check so record it
 		_time_last_arsp_fuse = _time_last_imu;
 
-		// update covariance matrix via Pnew = (I - KH)P = P - KHP
+		// apply covariance correction via P_new = (I -K*H)*P
+		// first calculate expression for KHP
+		// then calculate P - KHP
 		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that...
-				KH[row][column] = Kfusion[row] * H_TAS[column];
-			}
+			KH[row][4] = Kfusion[row] * H_TAS[4];
+			KH[row][5] = Kfusion[row] * H_TAS[5];
+			KH[row][6] = Kfusion[row] * H_TAS[6];
+			KH[row][22] = Kfusion[row] * H_TAS[22];
+			KH[row][23] = Kfusion[row] * H_TAS[23];
 		}
 
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column < _k_num_states; column++) {
-				for (unsigned i = 0; i < _k_num_states; i++) { // Check if this is correct matrix multiplication!
-					KHP[row][column] += KH[row][i] * P[i][column];
-				}
+				float tmp = KH[row][4] * P[4][column];
+				tmp += KH[row][5] * P[5][column];
+				tmp += KH[row][6] * P[6][column];
+				tmp += KH[row][22] * P[22][column];
+				tmp += KH[row][23] * P[23][column];
+				KHP[row][column] = tmp;
 			}
 		}
 
@@ -208,3 +215,17 @@ void Ekf::get_wind_velocity(float *wind)
 	wind[0] = _state.wind_vel(0);
 	wind[1] = _state.wind_vel(1);
 }
+
+/*
+ * Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
+*/
+void Ekf::resetWindStates()
+{
+	// get euler yaw angle
+	matrix::Euler<float> euler321(_state.quat_nominal);
+	float euler_yaw = euler321(2);
+
+	// estimate wind assuming zero sideslip
+	_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
+	_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
+}

EKF/control.cpp

@@ -738,20 +738,24 @@ void Ekf::controlRangeFinderFusion()
 
 void Ekf::controlAirDataFusion()
 {
-	// TODO This is just to get the logic inside but we will only start fusion once we tested this again
-	//if (_tas_data_ready) {
-	if (false) {
-		fuseAirspeed();
-
-	}
-
-	// control airspeed fusion - TODO move to a function
-	// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
+	// control activation and initialisation/reset of wind states
+	// wind states are required to do airspeed fusion
 	if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
+		// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
 		_control_status.flags.wind = false;
 
-	} else {
+	}
+
+	if (_tas_data_ready) {
+		// if we have airspeed data to fuse and winds states are inactive, then
+		// they need to be activated and the corresponding states and covariances reset
+		if (!_control_status.flags.wind) {
 			_control_status.flags.wind = true;
+			resetWindStates();
+			resetWindCovariance();
+		}
+
+		fuseAirspeed();
 
 	}
 }

EKF/covariance.cpp

@@ -563,50 +563,6 @@ void Ekf::predictCovariance()
 
 	// Don't do covariance prediction on wind states unless we are using them
 	if (_control_status.flags.wind) {
-		// Check if we have just transitioned to using wind states and set the variances accordingly
-		if (!_control_status_prev.flags.wind) {
-			// simple initialisation of wind states: calculate wind component along the forward axis
-			// of the plane.
-			
-			matrix::Euler<float> euler(_output_new.quat_nominal);
-			float heading = euler(2);
-
-			// ground speed component in the xy plane projected onto the directon the plane is heading to
-			float ground_speed_xy_nose = _output_new.vel(0) * cosf(heading) + _output_new.vel(1) * sinf(heading);
-			airspeedSample tmp = _airspeed_buffer.get_newest();
-			float airspeed = tmp.true_airspeed;
-
-			// check if the calculation is well conditioned:
-			// our airspeed measurement is at least as hight as our down velocity and the plane is moving forward
-			if (airspeed > fabsf(_output_new.vel(2)) && ground_speed_xy_nose > 0) {
-	
-				float ground_speed = sqrtf(_output_new.vel(0) * _output_new.vel(0) + _output_new.vel(1) * _output_new.vel(1) + _output_new.vel(2) * _output_new.vel(2));
-				
-				// wind magnitude in the direction the plane is
-				float wind_magnitude = ground_speed_xy_nose -  sqrtf(airspeed *  airspeed - _output_new.vel(2) * _output_new.vel(2));
-
-				// determine direction of wind
-				float wind_sign = 1;
-				if (airspeed < ground_speed) {
-					// wind is in nose direction
-					wind_sign = 1;
-				} else {
-					wind_sign = -1;
-				}
-
-				_state.wind_vel(0) = cosf(heading) * wind_magnitude * wind_sign;
-				_state.wind_vel(1) = sinf(heading) * wind_magnitude * wind_sign;
-
-			} else {
-				// calculation is badly conditioned, just set wind states to zero
-				_state.wind_vel.setZero();
-			}
-
-			// initialise diagonal of covariance matrix for the wind velocity states
-			for (uint8_t index = 22; index <= 23; index++) {
-				P[index][index] = sq(5.0f);
-			}
-		}
 
 		// calculate variances and upper diagonal covariances for wind states
 		nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10];
@@ -794,3 +750,35 @@ void Ekf::resetMagCovariance()
 		P[rc_index][rc_index] = sq(_params.mag_noise);
 	}
 }
+
+void Ekf::resetWindCovariance()
+{
+	// set the wind  covariance terms to zero
+	zeroRows(P,22,23);
+	zeroCols(P,22,23);
+
+	// calculate the wind speed and bearing
+	float spd = sqrtf(sq(_state.wind_vel(0))+sq(_state.wind_vel(1)));
+	float yaw = atan2f(_state.wind_vel(1),_state.wind_vel(0));
+
+	// calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle
+	// used to calculate the initial wind speed
+	float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
+	float R_yaw = sq(0.1745f);
+
+	// calculate the variance and covariance terms for the wind states
+	float cos_yaw = cosf(yaw);
+	float sin_yaw = sinf(yaw);
+	float cos_yaw_2 = sq(cos_yaw);
+	float sin_yaw_2 = sq(sin_yaw);
+	float sin_cos_yaw = sin_yaw*cos_yaw;
+	float spd_2 = sq(spd);
+	P[22][22] = R_yaw*spd_2*sin_yaw_2 + R_spd*cos_yaw_2;
+	P[22][23] = - R_yaw*sin_cos_yaw*spd_2 + R_spd*sin_cos_yaw;
+	P[23][22] = P[22][23];
+	P[23][23] = R_yaw*spd_2*cos_yaw_2 + R_spd*sin_yaw_2;
+
+	// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
+	P[22][22] += P[4][4];
+	P[23][23] += P[5][5];
+}

EKF/ekf.h

@@ -430,4 +430,10 @@ private:
 	// perform a limited reset of the magnetic field state covariances
 	void resetMagCovariance();
 
+	// perform a limited reset of the wind state covariances
+	void resetWindCovariance();
+
+	// perform a reset of the wind states
+	void resetWindStates();
+
 };

matlab/scripts/Inertial Nav EKF/polar2cart_cov.m

@@ -0,0 +1,8 @@
+clear all;
+syms spd yaw real;
+syms R_spd R_yaw real;
+vx = spd*cos(yaw);
+vy = spd*sin(yaw);
+Tpc = jacobian([vx;vy],[spd;yaw]);
+R_polar = [R_spd 0;0 R_yaw];
+R_cartesian = Tpc*R_polar*Tpc';