INS Simulation: Updates to allow setting separate pos and vel variance

Author: peabody124

matlab/ins/insgps.c

@@ -136,14 +136,14 @@ void INSPosVelReset(float pos[3], float vel[3])
 	X[5] = vel[2];	
 }
 
-void INSSetPosVelVar(float PosVar)
+void INSSetPosVelVar(float PosVar, float VelVar)
 {
 	R[0] = PosVar;
 	R[1] = PosVar;
 	R[2] = PosVar;
-	R[3] = PosVar;
-	R[4] = PosVar;
-//    R[5] = PosVar;  // Don't change vertical velocity, not measured
+	R[3] = VelVar;
+	R[4] = VelVar;
+    R[5] = VelVar;  // Don't change vertical velocity, not measured
 }
 
 void INSSetGyroBias(float gyro_bias[3])
@@ -238,7 +238,7 @@ void BaroCorrection(float baro)
 void GpsCorrection(float Pos[3], float Vel[3])
 {
     INSCorrection(zeros, Pos, Vel, zeros[0],
-		      POS_SENSORS); // | HORIZ_SENSORS | VERT_SENSORS);
+		      47);
 }
 
 

matlab/ins/insgps.h

@@ -53,7 +53,7 @@ void INSGPSInit();
 void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
 void INSCovariancePrediction(float dT);
 
-void INSSetPosVelVar(float PosVar);
+void INSSetPosVelVar(float PosVar, float VelVar);
 void INSSetGyroBias(float gyro_bias[3]);
 void INSSetAccelVar(float accel_var[3]);
 void INSSetGyroVar(float gyro_var[3]);

matlab/ins/insgps_ml.c

@@ -137,11 +137,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
 		VelBaroCorrection(vel_data, baro_data);
 	} else if (mlStringCompare(prhs[0], "INSSetPosVelVar")) {
 		float pos_var;
-		if((nrhs != 2) || !mlGetFloatArray(prhs[1], &pos_var, 1)) {
+        float vel_var;
+		if((nrhs != 3) || !mlGetFloatArray(prhs[1], &pos_var, 1) ||
+                !mlGetFloatArray(prhs[2], &vel_var, 1)) {
 			mexErrMsgTxt("Error with input parameters\n");
 			return;
 		}
-		INSSetPosVelVar(pos_var);
+		INSSetPosVelVar(pos_var, vel_var);
 	} else if (mlStringCompare(prhs[0], "INSSetGyroBias")) {
 		float gyro_bias[3];
 		if((nrhs != 2) || !mlGetFloatArray(prhs[1], gyro_bias, 3)) {