Merge remote-tracking branch 'upstream/mtecs_estimator' into navigator_rewrite_estimator

src/modules/ekf_att_pos_estimator/estimator_23states.cpp

@@ -2474,10 +2474,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     magstate.R_MAG = sq(magMeasurementSigma);
     magstate.DCM = DCM;
 
+    // Calculate position from gps measurement
+    float posNEDInit[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
+    calcposNED(posNEDInit, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
+
     // write to state vector
     for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
     for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
-    for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel
+    // positions:
+    states[7] = posNEDInit[0];
+    states[8] = posNEDInit[1];
+    states[9] = posNEDInit[2];
+    for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
     states[16] = initMagNED.x; // Magnetic Field North
     states[17] = initMagNED.y; // Magnetic Field East
     states[18] = initMagNED.z; // Magnetic Field Down