matlab/ins/insgps_ml.c

   1 #include "math.h"
   2 #include "mex.h"
   3 #include "insgps.h"
   4 #include "string.h"
   5 #include "stdint.h"
   6 #include "stdbool.h"
   7
   8 bool mlStringCompare(const mxArray * mlVal, char * cStr);
   9 bool mlGetFloatArray(const mxArray * mlVal, float * dest, int numel);
  10
  11 // constants/macros/typdefs
  12 #define NUMX 13                 // number of states, X is the state vector
  13 #define NUMW 9                  // number of plant noise inputs, w is disturbance noise vector
  14 #define NUMV 10                 // number of measurements, v is the measurement noise vector
  15 #define NUMU 6                  // number of deterministic inputs, U is the input vector
  16
  17 extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX];       // linearized system matrices
  18 extern float Be[3];                     // local magnetic unit vector in NED frame
  19 extern float P[NUMX][NUMX], X[NUMX];    // covariance matrix and state vector
  20 extern float Q[NUMW], R[NUMV];          // input noise and measurement noise variances
  21 extern float K[NUMX][NUMV];             // feedback gain matrix
  22 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
  23 {
  24         char * function_name;
  25         float accel_data[3];
  26         float gyro_data[3];
  27         float mag_data[3];
  28         float pos_data[3];
  29         float vel_data[3];
  30         float baro_data;
  31         float dT;
  32
  33         //All code and internal function calls go in here!
  34         if(!mxIsChar(prhs[0])) {
  35                 mexErrMsgTxt("First parameter must be name of a function\n");
  36                 return;
  37         }
  38
  39         if(mlStringCompare(prhs[0], "INSGPSInit")) {
  40                 INSGPSInit();
  41         } else  if(mlStringCompare(prhs[0], "INSStatePrediction")) {
  42
  43                 if(nrhs != 4) {
  44                         mexErrMsgTxt("Incorrect number of inputs for state prediction\n");
  45                         return;
  46                 }
  47
  48                 if(!mlGetFloatArray(prhs[1], gyro_data, 3) ||
  49                         !mlGetFloatArray(prhs[2], accel_data, 3) ||
  50                         !mlGetFloatArray(prhs[3], &dT, 1))
  51                         return;
  52
  53                 INSStatePrediction(gyro_data, accel_data, dT);
  54                 INSCovariancePrediction(dT);
  55         } else  if(mlStringCompare(prhs[0], "INSFullCorrection")) {
  56
  57                 if(nrhs != 5) {
  58                         mexErrMsgTxt("Incorrect number of inputs for correction\n");
  59                         return;
  60                 }
  61
  62                 if(!mlGetFloatArray(prhs[1], mag_data, 3) ||
  63                                 !mlGetFloatArray(prhs[2], pos_data, 3) ||
  64                                 !mlGetFloatArray(prhs[3], vel_data ,3) ||
  65                                 !mlGetFloatArray(prhs[4], &baro_data, 1)) {
  66                         mexErrMsgTxt("Error with the input parameters\n");
  67                         return;
  68                 }
  69
  70                 FullCorrection(mag_data, pos_data, vel_data, baro_data);
  71         } else  if(mlStringCompare(prhs[0], "INSMagCorrection")) {
  72                 if(nrhs != 2) {
  73                         mexErrMsgTxt("Incorrect number of inputs for correction\n");
  74                         return;
  75                 }
  76
  77                 if(!mlGetFloatArray(prhs[1], mag_data, 3)) {
  78                         mexErrMsgTxt("Error with the input parameters\n");
  79                         return;
  80                 }
  81
  82                 MagCorrection(mag_data);
  83     } else      if(mlStringCompare(prhs[0], "INSBaroCorrection")) {
  84                 if(nrhs != 2) {
  85                         mexErrMsgTxt("Incorrect number of inputs for correction\n");
  86                         return;
  87                 }
  88
  89                 if(!mlGetFloatArray(prhs[1], &baro_data, 1)) {
  90                         mexErrMsgTxt("Error with the input parameters\n");
  91                         return;
  92                 }
  93
  94                 BaroCorrection(baro_data);
  95         } else  if(mlStringCompare(prhs[0], "INSMagVelBaroCorrection")) {
  96
  97                 if(nrhs != 4) {
  98                         mexErrMsgTxt("Incorrect number of inputs for correction\n");
  99                         return;
 100                 }
 101
 102                 if(!mlGetFloatArray(prhs[1], mag_data, 3) ||
 103                                 !mlGetFloatArray(prhs[2], vel_data ,3) ||
 104                                 !mlGetFloatArray(prhs[3], &baro_data, 1)) {
 105                         mexErrMsgTxt("Error with the input parameters\n");
 106                         return;
 107                 }
 108
 109                 MagVelBaroCorrection(mag_data, vel_data, baro_data);
 110         } else  if(mlStringCompare(prhs[0], "INSGpsCorrection")) {
 111
 112                 if(nrhs != 3) {
 113                         mexErrMsgTxt("Incorrect number of inputs for correction\n");
 114                         return;
 115                 }
 116
 117                 if(!mlGetFloatArray(prhs[1], pos_data, 3) ||
 118                                 !mlGetFloatArray(prhs[2], vel_data ,3)) {
 119                         mexErrMsgTxt("Error with the input parameters\n");
 120                         return;
 121                 }
 122
 123                 GpsCorrection(pos_data, vel_data);
 124         } else  if(mlStringCompare(prhs[0], "INSVelBaroCorrection")) {
 125
 126                 if(nrhs != 3) {
 127                         mexErrMsgTxt("Incorrect number of inputs for correction\n");
 128                         return;
 129                 }
 130
 131                 if(!mlGetFloatArray(prhs[1], vel_data, 3) ||
 132                                 !mlGetFloatArray(prhs[2], &baro_data, 1)) {
 133                         mexErrMsgTxt("Error with the input parameters\n");
 134                         return;
 135                 }
 136
 137                 VelBaroCorrection(vel_data, baro_data);
 138         } else if (mlStringCompare(prhs[0], "INSSetPosVelVar")) {
 139                 float pos_var;
 140                 if((nrhs != 2) || !mlGetFloatArray(prhs[1], &pos_var, 1)) {
 141                         mexErrMsgTxt("Error with input parameters\n");
 142                         return;
 143                 }
 144                 INSSetPosVelVar(pos_var);
 145         } else if (mlStringCompare(prhs[0], "INSSetGyroBias")) {
 146                 float gyro_bias[3];
 147                 if((nrhs != 2) || !mlGetFloatArray(prhs[1], gyro_bias, 3)) {
 148                         mexErrMsgTxt("Error with input parameters\n");
 149                         return;
 150                 }
 151                 INSSetGyroBias(gyro_bias);
 152         } else if (mlStringCompare(prhs[0], "INSSetAccelVar")) {
 153                 float accel_var[3];
 154                 if((nrhs != 2) || !mlGetFloatArray(prhs[1], accel_var, 3)) {
 155                         mexErrMsgTxt("Error with input parameters\n");
 156                         return;
 157                 }
 158                 INSSetAccelVar(accel_var);
 159         } else if (mlStringCompare(prhs[0], "INSSetGyroVar")) {
 160                 float gyro_var[3];
 161                 if((nrhs != 2) || !mlGetFloatArray(prhs[1], gyro_var, 3)) {
 162                         mexErrMsgTxt("Error with input parameters\n");
 163                         return;
 164                 }
 165                 INSSetGyroVar(gyro_var);
 166         } else if (mlStringCompare(prhs[0], "INSSetMagNorth")) {
 167                 float mag_north[3];
 168                 float Bmag;
 169                 if((nrhs != 2) || !mlGetFloatArray(prhs[1], mag_north, 3)) {
 170                         mexErrMsgTxt("Error with input parameters\n");
 171                         return;
 172                 }
 173                 Bmag = sqrt(mag_north[0] * mag_north[0] + mag_north[1] * mag_north[1] +
 174                                 mag_north[2] * mag_north[2]);
 175                 mag_north[0] = mag_north[0] / Bmag;
 176                 mag_north[1] = mag_north[1] / Bmag;
 177                 mag_north[2] = mag_north[2] / Bmag;
 178
 179                 INSSetMagNorth(mag_north);
 180         } else if (mlStringCompare(prhs[0], "INSSetMagVar")) {
 181                 float mag_var[3];
 182                 if((nrhs != 2) || !mlGetFloatArray(prhs[1], mag_var, 3)) {
 183                         mexErrMsgTxt("Error with input parameters\n");
 184                         return;
 185                 }
 186                 INSSetMagVar(mag_var);
 187         } else if (mlStringCompare(prhs[0], "INSSetState")) {
 188         int i;
 189                 float new_state[NUMX];
 190                 if((nrhs != 2) || !mlGetFloatArray(prhs[1], new_state, NUMX)) {
 191                         mexErrMsgTxt("Error with input parameters\n");
 192                         return;
 193                 }
 194                 for(i = 0; i < NUMX; i++)
 195                         X[i] = new_state[i];
 196         } else {
 197                 mexErrMsgTxt("Unknown function");
 198         }
 199
 200         if(nlhs > 0) {
 201                 // return current state vector
 202                 double * data_out;
 203                 int i;
 204
 205                 plhs[0] = mxCreateDoubleMatrix(1,13,0);
 206                 data_out = mxGetData(plhs[0]);
 207                 for(i = 0; i < NUMX; i++)
 208                         data_out[i] = X[i];
 209         }
 210
 211         if(nlhs > 1) {
 212                 //return covariance estimate
 213                 double * data_copy = mxCalloc(NUMX*NUMX, sizeof(double));
 214                 int i, j, k;
 215
 216                 plhs[1] = mxCreateDoubleMatrix(13,13,0);
 217                 for(i = 0; i < NUMX; i++)
 218                         for(j = 0; j < NUMX; j++)
 219                         {
 220                                 data_copy[j + i * NUMX] = P[j][i];
 221                         }
 222
 223                 mxSetData(plhs[1], data_copy);
 224         }
 225
 226         if(nlhs > 2) {
 227                 //return covariance estimate
 228                 double * data_copy = mxCalloc(NUMX*NUMV, sizeof(double));
 229                 int i, j, k;
 230
 231                 plhs[2] = mxCreateDoubleMatrix(NUMX,NUMV,0);
 232                 for(i = 0; i < NUMX; i++)
 233                         for(j = 0; j < NUMV; j++)
 234                         {
 235                                 data_copy[j + i * NUMX] = K[i][j];
 236                         }
 237
 238                 mxSetData(plhs[2], data_copy);
 239         }
 240         return;
 241 }
 242
 243 bool mlGetFloatArray(const mxArray * mlVal, float * dest, int numel) {
 244         if(!mxIsNumeric(mlVal) || (!mxIsDouble(mlVal) && !mxIsSingle(mlVal)) || (mxGetNumberOfElements(mlVal) != numel)) {
 245                 mexErrMsgTxt("Data misformatted (either not double or not the right number)");
 246                 return false;
 247         }
 248
 249         if(mxIsSingle(mlVal)) {
 250                 memcpy(dest,mxGetData(mlVal),numel*sizeof(*dest));
 251         } else {
 252                 int i;
 253                 double * data_in = mxGetData(mlVal);
 254                 for(i = 0; i < numel; i++)
 255                         dest[i] = data_in[i];
 256         }
 257
 258         return true;
 259 }
 260
 261 bool mlStringCompare(const mxArray * mlVal, char * cStr) {
 262         int i;
 263         char * mlCStr = 0;
 264         bool val = false;
 265         int strLen = mxGetNumberOfElements(mlVal);
 266
 267         mlCStr = mxCalloc((1+strLen),  sizeof(*mlCStr));
 268         if(!mlCStr)
 269                 return false;
 270
 271         if(mxGetString(mlVal, mlCStr, strLen+1))
 272                 goto cleanup;
 273
 274         for(i = 0; i < strLen; i++) {
 275                 if(mlCStr[i] != cStr[i])
 276                         goto cleanup;
 277         }
 278
 279         if(cStr[i] == '\0')
 280                 val = true;
 281
 282 cleanup:
 283         if(mlCStr) {
 284                 mxFree(mlCStr);
 285                 mlCStr = 0;
 286         }
 287         return val;
 288 }