system Identification using MEX C++

I am trying to run a parameter estimation simulation:

When I try to run the program using the command:

compare(z,nlgr);

I get the error:

??? Error using ==> snls at 270
lsqnonlin cannot continue: user function is returning Inf or NaN values.

Error in ==> lsqncommon at 285
    [x,FVAL,LAMBDA,JACOB,EXITFLAG,OUTPUT,msg]=...

Error in ==> lsqnonlin at 172
[x,Resnorm,FVAL,EXITFLAG,OUTPUT,LAMBDA,JACOB] = ...

Error in ==> idestimatorpack.lsqnonlin.minimize at 24
[xnew, resnorm, residual, exitflag, output] = ...

Error in ==> idnlgrey.pem at 161
    optiminfo = minimize(Estimator);

Error in ==> idnlgrey.predict at 145
        nlsys = pem(getexp(data, i), nlsys);

Error in ==> utcompare at 566
            [yh{ksys}, x01{ksys}] = predict(th, z1, m, ksysinit);

Error in ==> idnlmodel.compare at 114
    utcompare(v{:});

I have just altered the code from the missile_c iddemo12 file to suit my needs. Could someone please help me in this final issue I have? - the file PedalDatad is already in a suitable format

Thanks

-------------------------------------------------------------------

My Mex file is this:

/ * Include libraries. */ #include "mex.h" #include math.h

/* Specify the number of outputs here. */
#define NY 4

/* State equations. */
void compute_dx(double *dx, double *x, double *u, double **p)
{
    /* Retrieve model parameters. */
    double *C, *S, *g;
    C = p[0];   /* Aerodynamic Lateral/directional derivatives.    */
    S = p[1];   /* Trim speed. */
    g = p[2];   /* gravity.  */

      /* x[0]: lateral velocity. */
      /* x[1]: Roll rate. */
      /* x[2]: Yaw rate. */
      /* x[3]: Roll angle. */
      dx[0] = -S[0]*x[2]+g[0]*x[3]+C[0]*x[0]+C[1]*x[1]+C[2]*x[2]+C[9]*u[0]+C[10]*u[1];
      dx[1] = C[3]*x[0]+C[4]*x[1]+C[5]*x[2]+C[11]*u[0]+C[12]*u[1];
      dx[2] = C[6]*x[0]+C[7]*x[1]+C[8]*x[2]+C[13]*u[0]+C[14]*u[1];
      dx[3] = x[1];
  }

/* Output equations. */
void compute_y(double *y, double *x, double *u, double **p)
{
    /* Retrieve model parameters. */
    double *C, *S, *g;
    C = p[0];   /* Aerodynamic Lateral/directional derivatives.    */
    S = p[1];   /* Trim speed. */
    g = p[2];   /* gravity.  */                 

      /* x[0]: lateral velocity. */
      /* x[1]: Roll rate. */
      /* x[2]: Yaw rate. */
      /* x[3]: Roll angle. */
      y[0] = x[0];
      y[1] = x[1];
      y[2] = x[2];
      y[3] = x[3];
  }

/*----------------------------------------------------------------------- *
   DO NOT MODIFY THE CODE BELOW UNLESS YOU NEED TO PASS ADDITIONAL
   INFORMATION TO COMPUTE_DX AND COMPUTE_Y

     To add extra arguments to compute_dx and compute_y (e.g., size
     information), modify the definitions above and calls below.
   *-----------------------------------------------------------------------*/

void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
    /* Declaration of input and output arguments. */
    double *x, *u, **p, *dx, *y, *t;
    int     i, np, nu, nx;
    const mxArray *auxvar = NULL; /* Cell array of additional data. */

      if (nrhs < 3) {
          mexErrMsgIdAndTxt("IDNLGREY:ODE_FILE:InvalidSyntax",
          "At least 3 inputs expected (t, u, x).");
      }

      /* Determine if auxiliary variables were passed as last input.  */
      if ((nrhs > 3) && (mxIsCell(prhs[nrhs-1]))) {
          /* Auxiliary variables were passed as input. */
          auxvar = prhs[nrhs-1];
          np = nrhs - 4; /* Number of parameters (could be 0). */
      } else {
          /* Auxiliary variables were not passed. */
          np = nrhs - 3; /* Number of parameters. */
      }

      /* Determine number of inputs and states. */
      nx = mxGetNumberOfElements(prhs[1]); /* Number of states. */
      nu = mxGetNumberOfElements(prhs[2]); /* Number of inputs. */

      /* Obtain double data pointers from mxArrays. */
      t = mxGetPr(prhs[0]);  /* Current time value (scalar). */
      x = mxGetPr(prhs[1]);  /* States at time t. */
      u = mxGetPr(prhs[2]);  /* Inputs at time t. */

      p = mxCalloc(np, sizeof(double*));
      for (i = 0; i < np; i++) {
          p[i] = mxGetPr(prhs[3+i]); /* Parameter arrays. */
      }

      /* Create matrix for the return arguments. */
      plhs[0] = mxCreateDoubleMatrix(nx, 1, mxREAL);
      plhs[1] = mxCreateDoubleMatrix(NY, 1, mxREAL);
      dx      = mxGetPr(plhs[0]); /* State derivative values. */
      y       = mxGetPr(plhs[1]); /* Output values. */

      /*
        Call the state and output update functions.

        Note: You may also pass other inputs that you might need,
        such as number of states (nx) and number of parameters (np).
        You may also omit unused inputs (such as auxvar).

        For example, you may want to use orders nx and nu, but not time (t)
        or auxiliary data (auxvar). You may write these functions as:
            compute_dx(dx, nx, nu, x, u, p);
            compute_y(y, nx, nu, x, u, p);
      */

      /* Call function for state derivative update. */
      compute_dx(dx, x, u, p);

      /* Call function for output update. */
      compute_y(y, x, u, p);

      /* Clean up. */
      mxFree(p);
  }

*mfile is this:

% As a first action we load the available missile data:
load Dedropped_out

z = PedalDatad; %initialise data with PedalDatad - which has already had trends removed and in iddata form
set(z, 'InputName', {'Stick Position' 'Pedal Position'}, ...
       'InputUnit', { '%' '%'});
set(z, 'OutputName', {'Lateral velocity component' 'Roll Rate' ...
                        'Yaw Rate' 'Roll Angle'}, ...
       'OutputUnit', { 'm/s' 'rad/s' 'rad/s' 'rad'});
set(z, 'Tstart', 0, 'TimeUnit', 's');

% The input data is shown in a figure

figure('Name', [z.Name ': input data']);
for i = 1:z.Nu
   subplot(z.Nu/2, 2, i);
   plot(z(:, [], i));
   title(['Input #' num2str(i) ': ' z.InputName{i}]);
   xlabel('');
   axis('tight');
   if (i > z.Nu-2)
       xlabel([z.Domain ' (' z.TimeUnit ')']);
   end
end

% The output data is shown in another figure.
figure('Name', [z.Name ': output data']);
for i = 1:z.Ny
   subplot(z.Ny, 1, i);
   plot(z(:, i, []));
   title(['Output #' num2str(i) ': ' z.OutputName{i}]);
   xlabel('');
   axis('tight');
end
xlabel([z.Domain ' (' z.TimeUnit ')']);

Parameters = struct('Name', {'Aerodynamic Lateral/directional derivatives'       ... % C, 15-by-1 vector.
                             'Trim speed'    ... % S, scalar.
                             'gravity'},     ... % g, scalar.
                    'Unit', {['-, -, -, -, '  ...
                              '-, -, -, -, '  ...
                              '-, -, -, -, '  ...
                              '-, -, -']      ...
                              'm/s' 'm/s/s'},   ...
                    'Value',   {[0;0;0;0;0;0;0;0;0;0;0;0;0;0;0]  ...
                                55 9.8056},            ...
                    'Minimum', {-Inf(15, 1) -Inf -Inf}, ... % Ignore constraints.
                    'Maximum', {Inf(15, 1) Inf Inf},         ... % Ignore constraints.
                    'Fixed',   {false(15, 1) true true});

%                                     'Value',   {[0.148; -1.661; 34.176; 0.047; -2.407; 0.169; 0.039; -0.214; -0.426; -0.009; 0.187; 0.069; -0.003; 0.02; 0.006]  ...
%                                 55 9.8056}, 

% We also define the 5 states of the missile model structure in the same
% manner:
InitialStates = struct('Name',    {'lateral velocity'        ...
                                   'Roll rate'        ...
                                   'Yaw rate'        ...
                                   'Roll angle'}, ...
                       'Unit',    {'m/s' 'rad/s' 'rad/s' 'rad'},   ...
                       'Value',   {0 0 0 0},                             ...
                       'Minimum', {-Inf -Inf -Inf -Inf},              ... % Ignore constraints.
                       'Maximum', {Inf Inf Inf Inf},                   ... % Ignore constraints.
                       'Fixed',   {true true true true});    

% The model file along with order, parameter and initial states data are
% now used to create an IDNLGREY object describing the missile system:
FileName     = 'flight_c';             % File describing the model structure.
Order        = [4 2 4];                % Model orders [ny nu nx].
Ts           = 0;                       % Time-continuous system.
nlgr = idnlgrey(FileName, Order, Parameters, InitialStates, Ts, ...
                'Name', 'Missile', 'TimeUnit', 's');                   

% The input and output signals of the missile system are next specified by
% simply copying the corresponding bookkeeping data from the IDDATA object:            
set(nlgr, 'InputName', z.InputName, 'InputUnit', z.InputUnit);
set(nlgr, 'OutputName', z.OutputName, 'OutputUnit', z.OutputUnit);

Answer 1

[Rajiv Singh]

It seems your ODE file is returning nonfinite values of either y or dx for the current parameter values in model nlgr. To debug this, I would recommend that you use use the MATLAB file version (.m) for the ODE rather than the mex file. This you can do by modifying the missile_m.m file. Using the .m file will allow you to put breakpoints and see when/why the output arguments become nonfinite. You could then think of imposing suitable constraints/safeguards in the ODE equations to prevent this.

The MEX version only gains you speed. I would first get everything resolved using the .m ODE file before implementing the ODE in C.

Answer 2

[Henry]

Thanks Rajiv, I got it working now. The equation I was given was wrong.

The working set-up is in the Time-domain. I am now trying to get it to work in the frequency domain. Do you have any suggestions? I believe this can only work in the Time-Domain?

[Rajiv Singh]

The nonlinear grey box (idnlgrey) does not support frequency domain data. The linear grey box (idgrey) does support frequency domain data, which will of course only work if your system of equations is linear.

[Henry]

Is there another function in MATLAB that I can use to perform non-linear parametric system identification in the frequency domain?

[Rajiv Singh]

Look at Simulink Design Optimization. There is support for imposing frequency domain constraints on some linearized version of your nonlinear model represented in Simulink.

You could also consider writing your own data fitting method using Optimization Toolbox, if you can write frequency domain expressions for your model (using say, a harmonic balance approach if inputs are periodic; see e.g., http://www.mendeley.com/research/identification-nonlinear-viscoelastic-properties-flexible-polyurethane-foam/).