AP_NavEKF : Enable calc of wind velocity when not using airspeed sensing

This patch also cleans up the logic associated with use of the synthetic
sideslip measurement so that it can never be used for a non fly-forward
vehicle type

This patch adds functionality that initialises the wind-speed vector to the
reciprocal of the ground speed vector, and scaled to 6 m/s. On average this gives
a better initial wind velocity estimate on launch by assuming:

a) launch will be into wind
b) wind speed is equal to global average

It also helps prevent a headwind causing initial underestimation of airspeed
causing high autopilot gains and limit cycles on climb-out, until first
turn when the EKF is able to estimate the wind.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -761,9 +761,10 @@ void NavEKF::SelectBetaFusion()
     // synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero
     // it requires a stable wind estimate for best results and should not be used for aerobatic flight
     // we fuse synthetic sideslip measurements if:
-    // (we are not using a compass OR (we are dead-reckoning position AND using airspeed)) AND not on the ground AND enough time has lapsed since our last fusion AND
-    // (we have not fused magnetometer data on this time step OR the immediate fusion flag is set)
-    if ((!use_compass() || (!posHealth && useAirspeed())) && !onGround  && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
+    // we are a fly forward vehicle type AND NOT using a full range of sensors with healthy position
+    // AND NOT on the ground AND enough time has lapsed since our last fusion
+    // AND (we have not fused magnetometer data on this time step OR the immediate fusion flag is set)
+    if (_ahrs->get_fly_forward() && !(use_compass() && useAirspeed() && posHealth) && !onGround  && ((IMUmsec - BETAmsecPrev) >= _msecBetaAvg) && (!magFusePerformed || fuseMeNow)) {
         FuseSideslip();
         BETAmsecPrev = IMUmsec;
     }
@@ -2709,6 +2710,14 @@ void NavEKF::OnGroundCheck()
         if (!onGround && prevOnGround && !use_compass()) {
             alignYawGPS();
         }
+        // If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround
+        // we set the wind velocity to the reciprocal of the velocity vector and scale states so that the
+        // wind speed is equal to the 6m/s. This prevents gains being too high at the start
+        // of flight if launching into a headwind until the first turn when the EKF can form a  wind speed
+        // estimate
+        if (!onGround && prevOnGround && !useAirspeed() && _ahrs->get_fly_forward()) {
+            setWindVelStates();
+        }
     }
     prevOnGround = onGround;
 }
@@ -2795,9 +2804,9 @@ void NavEKF::CopyAndFixCovariances()
             }
         }
     }
-    // if we flying and are not using airspeed and are not using synthetic sideslip measurements, we want all the off-diagonals for the wind
+    // if we have a non-forward flight vehicle type and no airspeed sensor, we want the wind
     // states to remain zero and want to keep the old variances for these states
-    else if (!useAirspeed() && use_compass()) {
+    else if (!useAirspeed() && !_ahrs->get_fly_forward()) {
         // copy calculated variances we want to propagate
         for (uint8_t i=0; i<=13; i++) {
             P[i][i] = nextP[i][i];
@@ -3133,6 +3142,26 @@ void NavEKF::alignYawGPS()
     }
 }
 
+// This function is used to do a forced alignment of the wind velocity states so that they are set to the reciprocal of
+// the ground speed and scaled to 6 m/s. This is used when launching a fly-forward vehicle without an airspeed sensor
+// on the assumption that launch will be into wind and 6 is representative global average at height
+// http://maps.google.com/gallery/details?id=zJuaSgXp_WLc.kTBytKPmNODY&hl=en
+void NavEKF::setWindVelStates()
+{
+    float gndSpd = sqrtf(sq(states[4]) + sq(states[5]));
+    if (gndSpd > 4.0f) {
+        // set the wind states to be the reciprocal of the velocity and scale to 6 m/s
+        float scaleFactor = 6.0f / gndSpd;
+        states[14] = - states[4] * scaleFactor;
+        states[15] = - states[5] * scaleFactor;
+        // reinitialise the wind state covariances
+        zeroRows(P,14,15);
+        zeroCols(P,14,15);
+        P[14][14] = 64.0f;
+        P[15][15] = P[14][14];
+    }
+}
+
 // return the transformation matrix from XYZ (body) to NED axes
 void NavEKF::getRotationBodyToNED(Matrix3f &mat) const
 {

libraries/AP_NavEKF/AP_NavEKF.h

@@ -248,6 +248,12 @@ private:
     // force alignment of the yaw angle using GPS velocity data
     void alignYawGPS();
 
+    // Forced alignment of the wind velocity states so that they are set to the reciprocal of
+    // the ground speed and scaled to 6 m/s. This is used when launching a fly-forward vehicle without an airspeed sensor
+    // on the assumption that launch will be into wind and 6 is representative global average at height
+    // http://maps.google.com/gallery/details?id=zJuaSgXp_WLc.kTBytKPmNODY&hl=en
+    void setWindVelStates();
+
     // initialise the earth magnetic field states using declination and current attitude and magnetometer meaasurements
     // and return attitude quaternion
     Quaternion calcQuatAndFieldStates(float roll, float pitch);