diff --git a/commonFunctions/kalman/correctionGPS.m b/commonFunctions/kalman/correctionGPS.m index afdc61a5a6e7738603e9b3ed67bfbcac8e6fca47..50b9376e23548479d886df46b30980f8e880a68d 100644 --- a/commonFunctions/kalman/correctionGPS.m +++ b/commonFunctions/kalman/correctionGPS.m @@ -46,17 +46,17 @@ threshold = 10e-11; % output of the system multiplied by 1000 to deal with magnitude of order % of covariance -H11 = 1000/a; +H11 = 1/a; H12 = 0; -H21 = (1000000*x_pred(2)*sind(1000*lat0 + (1000*x_pred(1))/a))/(a*b*cosd(1000*lat0 + (1000*x_pred(1))/a)^2); -H22 = 1000/(b*cosd(1000*lat0 + (1000*x_pred(1))/a)); +H21 = (1*x_pred(2)*sind(1*lat0 + (1*x_pred(1))/a))/(a*b*cosd(1*lat0 + (1*x_pred(1))/a)^2); +H22 = 1/(b*cosd(1*lat0 + (1*x_pred(1))/a)); H = [ H11 H12 0 0 0 0; %Pre-allocation of gradient H21 H22 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0;]; %of the output function -R = sigma_GPS.*[1 1 max(1,vGPS)]; %VARIANCE MATRIX SCALED +R = sigma_GPS.*[1e-6 1e-6 max(1,vGPS)]; %VARIANCE MATRIX SCALED %TAKING INTO ACCOUNT %NUMBER OF SATELITES %AVAILABLE