Mercurial > hg > ltpda

comparison src/ltpda_ssmsim/ltpda_ssmsim.c @ 0:f0afece42f48

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Import.
author Daniele Nicolodi <nicolodi@science.unitn.it>
date Wed, 23 Nov 2011 19:22:13 +0100
parents
children
comparison
equal deleted inserted replaced
-1:000000000000 0:f0afece42f48
1 #include <math.h>
2 #include <stdio.h>
3 #include <stdlib.h>
4 #include <string.h>
5 #include <mex.h>
6
7 #include "matrix.h"
8 #include "version.h"
9 #include "ltpda_ssmsim.h"
10
11 #define DEBUG 0
12 /*
13 * A mex file to propagate an input signal with the given SS model
14 *
15 * M Hewitson 19-08-10
16 *
17 * $Id: ltpda_ssmsim.c,v 1.4 2010/08/23 11:56:09 ingo Exp $
18 */
19
20
21 /*
22 * function [y,lx] = ltpda_ssmsim(lastX, A.', Coutputs.', Cstates.', Baos.', Daos.', input);
23 */
24 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
25 {
26
27 /* outputs */
28 double *y, *yptr;
29
30 /* inputs */
31 double *input, *iptr;
32 double *Daos, *Dptr;
33 double *Baos, *Bptr;
34 double *Cstates;
35 double *Coutputs, *Coptr;
36 double *A, *Aptr;
37 double *lastX;
38 double *SSini;
39 double *tmpX;
40
41 mwSize Ninputs, Nsamples, Nstates, Nstatesout, Noutputs;
42 int kk,jj,ll;
43 int mm,nn;
44 int ki;
45
46
47 /* parse input functions */
48
49 if( (nrhs == 0) || (nlhs == 0) )
50 {
51 print_usage(VERSION);
52 }
53
54 if( (nrhs == 7) && (nlhs == 2) )/* let's go */
55 {
56 /*----------------- set inputs*/
57 SSini = mxGetPr(prhs[0]);
58 A = mxGetPr(prhs[1]);
59 Coutputs = mxGetPr(prhs[2]);
60 Cstates = mxGetPr(prhs[3]);
61 Baos = mxGetPr(prhs[4]);
62 Daos = mxGetPr(prhs[5]);
63 input = mxGetPr(prhs[6]);
64
65 Ninputs = mxGetM(prhs[4]);
66 Nsamples = mxGetN(prhs[6]);
67 Nstates = mxGetM(prhs[1]);
68 Nstatesout = mxGetM(prhs[3]);
69 Noutputs = mxGetN(prhs[2]);
70
71 #if DEBUG
72 mexPrintf("Ninputs: %d\n", Ninputs);
73 mexPrintf("Nsamples: %d\n", Nsamples);
74 mexPrintf("Nstates: %d\n", Nstates);
75 mexPrintf("Nstatesout: %d\n", Nstatesout);
76 mexPrintf("Noutputs: %d\n", Noutputs);
77
78 mexPrintf("N Coutputs: %d\n", mxGetNumberOfElements(prhs[2]));
79
80 mexPrintf("input: %dx%d\n", mxGetM(prhs[6]), mxGetN(prhs[6]));
81 mexPrintf("D: %dx%d\n", mxGetN(prhs[5]), mxGetM(prhs[5]));
82 #endif
83
84 /* output y */
85 plhs[0] = mxCreateDoubleMatrix(Noutputs, Nsamples, mxREAL);
86 y = mxGetPr(plhs[0]);
87
88 /* output state vector*/
89 plhs[1] = mxCreateDoubleMatrix(Nstates, 1, mxREAL);
90 lastX = mxGetPr(plhs[1]);
91
92 tmpX = (double*)calloc(Nstates, sizeof(double));
93 memcpy(lastX, SSini, Nstates*sizeof(double));
94
95 /* do the business */
96 yptr = &(y[0]);
97 for (kk=0; kk<Nsamples; kk++) {
98
99 ki = kk*Ninputs;
100
101 /* observation equation */
102 Coptr = &(Coutputs[0]);
103 Dptr = &(Daos[0]);
104 for (jj=0; jj<Noutputs; jj++) {
105 *yptr = 0.0;
106 for (ll=0; ll<Nstates; ll++) {
107 *yptr += *Coptr * lastX[ll] ;
108 Coptr++;
109 }
110 iptr = &(input[ki]);
111 for (ll=0; ll<Ninputs; ll++) {
112 *yptr += *Dptr * (*iptr);
113 Dptr++;
114 iptr++;
115 }
116 yptr++;
117 }
118
119 /* state propagation */
120 memcpy(tmpX, lastX, Nstates*sizeof(double));
121 Bptr = &(Baos[0]);
122 Aptr = &(A[0]);
123 for (jj=0; jj<Nstates; jj++) {
124 lastX[jj] = 0;
125 for (ll=0; ll<Nstates; ll++) {
126 lastX[jj] += *Aptr * tmpX[ll];
127 Aptr++;
128 }
129 iptr = &(input[ki]);
130 for (ll=0; ll<Ninputs; ll++) {
131 lastX[jj] += *Bptr * (*iptr);
132 Bptr++;
133 iptr++;
134 }
135 }
136
137
138 } /* end sample loop */
139
140
141 free(tmpX);
142
143
144 }
145 else
146 {
147 print_usage(VERSION);
148 }
149 }
150
151 void print_usage(char *version)
152 {
153 mexPrintf("ltpda_ssmsim version %s\n", version);
154 mexPrintf(" usage: [y,lx] = ltpda_ssmsim(lastX, A.', Coutputs.', Cstates.', Baos.', Daos.', input);");
155 mexErrMsgTxt("### incorrect usage");
156 }
157