matlab_utilities/Kalmanantec.cpp

MEX file that calculates an iteration of EKF only for antecedents.

MATLAB help:

function [Model, P, err] = Kalmanantec(Model, x, output, covariance, P)
% KALMANANTEC. Calculates an iteration of EKF only for antecedents.
%
%   [Model, P, err] = Kalmanantec(Model, x, output, covariance, P)
%
%   [Model, P, err] = Kalmanantec(Model, x, output, covariance, P, Phi)
%
% Arguments:
% 
%   Model -> This fuzzy model could be a '.txt' file, a '.fis' file,
%            or a 'FIS' variable from MATLAB Workspace.
%
%   x -> Current State vector, x(k).
%
%   output -> Real output of the system in the current iteration.
%
%   covariance -> Noise covariance matrix estimated from the hope operator.
%
%   P -> Covariance matrix of the filter.
%
%   Phi -> Jacobian matrix that relates the parameters to be set with the
%          following value of these parameters.
%          If not specified, Phi is assumed to be the identity matrix.
%
% See also Kalmanconseq, Kalmanfuz, activation, antec2mat, aproxjac, aproxlinear
%          conseq2mat, fis2txt, fuz2mat, fuzcomb, fuzeval, fuzjac, fuzlinear,
%          fuzprint, mat2antec, mat2conseq, mat2fuz, subsystem, txt2fis

Source code:

/*  Copyright (C) 2004-2011
  ANTONIO JAVIER BARRAGAN, antonio.barragan@diesia.uhu.es
  http://uhu.es/antonio.barragan

  Collaborators:
  JOSE MANUEL ANDUJAR, andujar@diesia.uhu.es
  MARIANO J. AZNAR, marianojose.aznar@alu.uhu.es

  DPTO. DE ING. ELECTRONICA, DE SISTEMAS INFORMATICOS Y AUTOMATICA
  ETSI, UNIVERSITY OF HUELVA (SPAIN)

  For more information, please contact with authors.
  
  This software is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This software is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "aux_matlab.hpp"
#include <flt/Kalman.hpp>

using namespace FLT;
using namespace TNT;

void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  // Reads and checks arguments
  if(nrhs<5 | nrhs>6)
    ERRORMSG(E_NumberArgIn)
  if (nlhs>3)
    ERRORMSG(E_NumberArgOut)
  for (size_t i=0;i<nrhs;i++)
    if (mxIsEmpty(prhs[i]) || mxIsNaN(*mxGetPr(prhs[i])))
      ERRORMSG(E_ArgNoValid)
  
  System Model;
  if (readModel(prhs[0],Model))
    ERRORMSG(E_Model)
  
  int m = Model.outputs();
  
  double *p_input = mxGetPr(prhs[1]);
  if (!p_input)
    ERRORMSG(E_NumberArgIn)
  int n = mxGetM(prhs[1]);
  if (n==1)
    n = mxGetN(prhs[1]);
  Array1D<double> input(n, p_input);

  double *p_output = mxGetPr(prhs[2]);
  if (!p_output)
    ERRORMSG(E_NumberArgIn)
  n = mxGetM(prhs[2]);
  if (n==1)
    n = mxGetN(prhs[2]);
  Array1D<double> output(n, p_output);

  double *p_covariance = mxGetPr(prhs[3]);
  if (!p_covariance)
    ERRORMSG(E_NumberArgIn)
  Array2D<double> covariance = col2row_major(p_covariance, m, m);

  n = mxGetN(prhs[4]);
  double *p_P = mxGetPr(prhs[4]);
  if (!p_P)
    ERRORMSG(E_NumberArgIn)
  Array2D<double> P = col2row_major(p_P, n, n);

  // Does the iteration
  Array1D<double> error;
  if (nrhs==5)
    error = KalmanAntec(Model, input, output, covariance, P);
  else
  {
    double *p_Phi = mxGetPr(prhs[5]);
    if (!p_Phi)
      ERRORMSG(E_NumberArgIn)
    Array2D<double> Phi = col2row_major(p_Phi, n, n);
    
    error = KalmanAntec(Model, input, output, covariance, P, Phi);
  }
  
  if (error.dim() == 0)
    ERRORMSG(E_InNoCoherent)

  // Generates outputs
  const char* names[]={"name","type","andMethod","orMethod","defuzzMethod","impMethod","aggMethod","input","output","rule"};
  int dim[] = {1,1};
  plhs[0] = mxCreateStructArray(2,dim,10,names);
  if(System2FIS(Model, plhs[0]))
  {
    mxDestroyArray(plhs[0]);
    ERRORMSG(E_CreateFIS)
  }
  
  if (nlhs>1)
  {
    plhs[1] = mxCreateDoubleMatrix(n,n,mxREAL);
    double *p_newP = mxGetPr(plhs[1]);
    if (!p_newP)
    {
      mxDestroyArray(plhs[0]);
      mxDestroyArray(plhs[1]);
      ERRORMSG(E_NumberArgOut)
    }
    row2col_major(P[0], p_newP, n, n);
    
    if (nlhs>2)
    {
      plhs[2] = mxCreateDoubleMatrix(m,1,mxREAL);
      double *p_error = mxGetPr(plhs[2]);
      if (!p_error)
      {
        mxDestroyArray(plhs[0]);
        mxDestroyArray(plhs[1]);
        mxDestroyArray(plhs[2]);
        ERRORMSG(E_NumberArgOut)
      }
      for (size_t i=0;i<m;i++)
      {
        *p_error = error[i];
        p_error++;
      }
    }
  }
}