GPS now give datum in geodetic coordinates

sensors/acquisition_Sys.m

@@ -75,6 +75,9 @@ OUTPUT:
                                                  sensorData.gps.positionMeasures(ii,2),...
                                                - sensorData.gps.positionMeasures(ii,3),...
                                                  14.8500); 
+                                             
+            [sp.gps(ii,1),sp.gps(ii,2),sp.gps(ii,3)]    = ned2geodetic(sp.gps(ii,1),sp.gps(ii,2),sp.gps(ii,3),s.lat0, s.lon0, s.z0,s.spheroid ,'degrees');                            
+           
             [sensorData.gps.velocityMeasures(ii,1),sensorData.gps.velocityMeasures(ii,2),sensorData.gps.velocityMeasures(ii,3)] =           ...
                                                  s.GPS_NEOM9N.sens( ...
                                                  sensorData.gps.velocityMeasures(ii,1),...

sensors/initSensors.m

-function [s, sensorTot]  = initSensors
+function [s, sensorTot]  = initSensors(lat0, lon0, z0)
 % Initialize all sensors
     
 % Author: Jan Hammelman
@@ -66,7 +66,10 @@ s.MAGN_LSM9DS1.transMatrix          =   diag([1 1 1]);          % axis transform
 s.GPS_NEOM9N = Sensor3D();                                      % lon, in degree lat in deree, alt in m
 s.GPS_NEOM9N.noiseVariance          =   2;                      % in m
 s.GPS_NEOM9N.transMatrix            =   diag([1 1 1]);          % axis transformation
-
+s.lat0                              =   lat0;
+s.lon0                              =   lon0;
+s.z0                                =   z0;
+s.spheroid                          =   wgs84Ellipsoid;
 % initial megnetometer sensor from IIS2MDC: TODO
 s.MAGN_IIS2MDC = Sensor3D();                                    % magnetic field in mgauss, temp should be in C°-25C°
 s.MAGN_IIS2MDC.maxMeasurementRange  =   49152;                  % in mgauss

simulator/configEuroc.m

@@ -144,6 +144,11 @@ settings.kalman.counter       =   0;
 settings.kalman.t_kalman      =   -1;                                      % Apogee detection timestamp
 settings.kalman.flag_apo      =   false;                                   % True when the apogee is detected
 
+settings.kalman.lat0          = settings.lat0;
+settings.kalman.lon0          = settings.lon0;
+settings.kalman.z0            = -settings.z0;
+settings.kalman.spheroid      = wgs84Ellipsoid;
+
 % Process noise covariance matrix for the linear dynamics
 settings.kalman.QLinear       =   1*...
                                  [0       0       0        0        0       0;

simulator/configRoccaraso.m

@@ -145,6 +145,11 @@ settings.kalman.counter       =   0;
 settings.kalman.t_kalman      =   -1;                                      % Apogee detection timestamp
 settings.kalman.flag_apo      =   false;                                   % True when the apogee is detected
 
+settings.kalman.lat0          = settings.lat0;
+settings.kalman.lon0          = settings.lon0;
+settings.kalman.z0            = -settings.z0;
+settings.kalman.spheroid      = wgs84Ellipsoid;
+
 % Process noise covariance matrix for the linear dynamics
 settings.kalman.QLinear       =   0.005*...
                                  [4       0       0        0        0       0;

simulator/std_run_HIL.m

@@ -87,7 +87,7 @@ addpath('../control');
 %% SENSORS DEFINITION
 addpath('../sensors');
 addpath('../sensors/data/MS580301BA01');
-[s, tot] = initSensors;
+[s, tot] = initSensors(settings.lat0, settings.lon0, settings.z0);
 
 %% MAGNETIC FIELD MODEL
 hmax = 6000;