Improve aesthetics of "Adjust state space expectation for removal of omxRaiseErrorf"
[openmx:openmx.git] / src / omxStateSpaceExpectation.cpp
1 /*
2  *  Copyright 2007-2014 The OpenMx Project
3  *
4  *  Licensed under the Apache License, Version 2.0 (the "License");
5  *  you may not use this file except in compliance with the License.
6  *  You may obtain a copy of the License at
7  *
8  *       http://www.apache.org/licenses/LICENSE-2.0
9  *
10  *   Unless required by applicable law or agreed to in writing, software
11  *   distributed under the License is distributed on an "AS IS" BASIS,
12  *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  *  See the License for the specific language governing permissions and
14  *  limitations under the License.
15  *
16  */
17
18
19 /***********************************************************
20 *
21 *  omxStateSpaceExpectation.c
22 *
23 *  Created: Michael D. Hunter   Date: 2012-10-28 20:07:36
24 *
25 *  Contains code to calculate the objective function for a
26 *   state space model.  Currently, this is done with a 
27 *   Kalman filter in separate Predict and Update steps.
28 *   Later this could be done with one of several Kalman 
29 *   filter-smoothers (a forward-backward algorithm).
30 *
31 **********************************************************/
32
33 #include "omxExpectation.h"
34 #include "omxBLAS.h"
35 #include "omxFIMLFitFunction.h"
36 #include "omxStateSpaceExpectation.h"
37
38
39 void omxCallStateSpaceExpectation(omxExpectation* ox, const char *, const char *) {
40     if(OMX_DEBUG) { mxLog("State Space Expectation Called."); }
41         omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);
42         
43         omxRecompute(ose->A);
44         omxRecompute(ose->B);
45         omxRecompute(ose->C);
46         omxRecompute(ose->D);
47         omxRecompute(ose->Q);
48         omxRecompute(ose->R);
49         
50         // Probably should loop through all the data here!!!
51         omxKalmanPredict(ose);
52         omxKalmanUpdate(ose);
53 }
54
55
56
57 void omxDestroyStateSpaceExpectation(omxExpectation* ox) {
58         
59         if(OMX_DEBUG) { mxLog("Destroying State Space Expectation."); }
60         
61         omxStateSpaceExpectation* argStruct = (omxStateSpaceExpectation*)(ox->argStruct);
62         
63         omxFreeMatrix(argStruct->r);
64         omxFreeMatrix(argStruct->s);
65         omxFreeMatrix(argStruct->z);
66         omxFreeMatrix(argStruct->x);
67         omxFreeMatrix(argStruct->y);
68         omxFreeMatrix(argStruct->K);
69         omxFreeMatrix(argStruct->P);
70         omxFreeMatrix(argStruct->S);
71         omxFreeMatrix(argStruct->Y);
72         omxFreeMatrix(argStruct->Z);
73         omxFreeMatrix(argStruct->det);
74         omxFreeMatrix(argStruct->covInfo);
75         omxFreeMatrix(argStruct->cov);
76         omxFreeMatrix(argStruct->means);
77         omxFreeMatrix(argStruct->smallC);
78         omxFreeMatrix(argStruct->smallD);
79         omxFreeMatrix(argStruct->smallR);
80         omxFreeMatrix(argStruct->smallr);
81         omxFreeMatrix(argStruct->smallK);
82         omxFreeMatrix(argStruct->smallS);
83         omxFreeMatrix(argStruct->smallY);
84 }
85
86
87 void omxPopulateSSMAttributes(omxExpectation *ox, SEXP algebra) {
88     if(OMX_DEBUG) { mxLog("Populating State Space Attributes.  Currently this does very little!"); }
89         
90 }
91
92
93
94
95 void omxKalmanPredict(omxStateSpaceExpectation* ose) {
96         if(OMX_DEBUG) { mxLog("Kalman Predict Called."); }
97         /* Creat local copies of State Space Matrices */
98         omxMatrix* A = ose->A;
99         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(A, "....State Space: A"); }
100         omxMatrix* B = ose->B;
101         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(B, "....State Space: B"); }
102         //omxMatrix* C = ose->C;
103         //omxMatrix* D = ose->D;
104         omxMatrix* Q = ose->Q;
105         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Q, "....State Space: Q"); }
106         //omxMatrix* R = ose->R;
107         //omxMatrix* r = ose->r;
108         //omxMatrix* s = ose->s;
109         omxMatrix* u = ose->u;
110         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(u, "....State Space: u"); }
111         omxMatrix* x = ose->x;
112         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x"); }
113         //omxMatrix* y = ose->y;
114         omxMatrix* z = ose->z;
115         //omxMatrix* K = ose->K;
116         omxMatrix* P = ose->P;
117         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P"); }
118         //omxMatrix* S = ose->S;
119         //omxMatrix* Y = ose->Y;
120         omxMatrix* Z = ose->Z;
121
122         /* x = A x + B u */
123         omxDGEMV(FALSE, 1.0, A, x, 0.0, z); // x = A x
124         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(z, "....State Space: z = A x"); }
125         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(A, "....State Space: A"); }
126         omxDGEMV(FALSE, 1.0, B, u, 1.0, z); // x = B u + x THAT IS x = A x + B u
127         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(z, "....State Space: z = A x + B u"); }
128         omxCopyMatrix(x, z); // x = z THAT IS x = A x + B u
129         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x = A x + B u"); }
130         
131         /* P = A P A^T + Q */
132         omxDSYMM(FALSE, 1.0, P, A, 0.0, Z); // Z = A P
133         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Z, "....State Space: Z = A P"); }
134         omxCopyMatrix(P, Q); // P = Q
135         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = Q"); }
136         omxDGEMM(FALSE, TRUE, 1.0, Z, A, 1.0, P); // P = Z A^T + P THAT IS P = A P A^T + Q
137         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = A P A^T + Q"); }
138 }
139
140
141 void omxKalmanUpdate(omxStateSpaceExpectation* ose) {
142         //TODO: Clean up this hack of function.
143         if(OMX_DEBUG) { mxLog("Kalman Update Called."); }
144         /* Creat local copies of State Space Matrices */
145         omxMatrix* C = ose->C;
146         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(C, "....State Space: C"); }
147         omxMatrix* D = ose->D;
148         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(D, "....State Space: D"); }
149         // omxMatrix* R = ose->R; //unused
150         omxMatrix* r = ose->r;
151         omxMatrix* s = ose->s;
152         omxMatrix* u = ose->u;
153         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(u, "....State Space: u"); }
154         omxMatrix* x = ose->x;
155         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x"); }
156         omxMatrix* y = ose->y;
157         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(y, "....State Space: y"); }
158         // omxMatrix* K = ose->K; //unused
159         omxMatrix* P = ose->P;
160         omxMatrix* S = ose->S;
161         // omxMatrix* Y = ose->Y; //unused
162         // omxMatrix* Cov = ose->cov; //unused
163         omxMatrix* Means = ose->means;
164         omxMatrix* Det = ose->det;
165         *Det->data = 0.0; // the value pointed to by Det->data is assigned to be zero
166         omxMatrix* smallC = ose->smallC;
167         omxMatrix* smallD = ose->smallD;
168         omxMatrix* smallr = ose->smallr;
169         omxMatrix* smallR = ose->smallR;
170         omxMatrix* smallK = ose->smallK;
171         omxMatrix* smallS = ose->smallS;
172         omxMatrix* smallY = ose->smallY;
173         int ny = y->rows;
174         int nx = x->rows;
175         int toRemoveSS[ny];
176         int numRemovesSS = 0;
177         int toRemoveNoneLat[nx];
178         int toRemoveNoneOne[1];
179         memset(toRemoveNoneLat, 0, sizeof(int) * nx);
180         memset(toRemoveNoneOne, 0, sizeof(int) * 1);
181         
182         omxMatrix* covInfo = ose->covInfo;
183         int info = 0; // Used for computing inverse for Kalman gain
184         
185         omxAliasMatrix(smallS, S); //re-alias every time expectation is called, at end delete the alias.
186         
187         /* Reset/Resample aliased matrices */
188         omxResetAliasedMatrix(smallC);
189         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C (Reset)"); }
190         omxResetAliasedMatrix(smallD);
191         omxResetAliasedMatrix(smallR);
192         omxResetAliasedMatrix(smallr);
193         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space: r (Reset)"); }
194         omxResetAliasedMatrix(smallK);
195         omxResetAliasedMatrix(smallS);
196         omxResetAliasedMatrix(smallY);
197         
198         /* r = r - C x - D u */
199         /* Alternatively, create just the expected value for the data row, x. */
200         omxDGEMV(FALSE, 1.0, smallC, x, 0.0, s); // s = C x
201         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(s, "....State Space: s = C x"); }
202         omxDGEMV(FALSE, 1.0, D, u, 1.0, s); // s = D u + s THAT IS s = C x + D u
203         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(s, "....State Space: s = C x + D u"); }
204         omxCopyMatrix(Means, s); // Means = s THAT IS Means = C x + D u
205         //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Means, "....State Space: Means"); }
206         omxTransposeMatrix(Means);
207         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Means, "....State Space: Means"); }
208         
209         //If entire data vector, y, is missing, then set residual, r, to zero.
210         //otherwise, compute residual.
211         memset(toRemoveSS, 0, sizeof(int) * ny);
212         for(int j = 0; j < y->rows; j++) {
213                 double dataValue = omxMatrixElement(y, j, 0);
214                 int dataValuefpclass = std::fpclassify(dataValue);
215                 if(dataValuefpclass == FP_NAN || dataValuefpclass == FP_INFINITE) {
216                         numRemovesSS++;
217                         toRemoveSS[j] = 1;
218                         omxSetMatrixElement(r, j, 0, 0.0);
219                 } else {
220                         omxSetMatrixElement(r, j, 0, (dataValue -  omxMatrixElement(s, j, 0)));
221                 }
222         }
223         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(r, "....State Space: Residual (Loop)"); }
224         /* Now compute the residual */
225         //omxCopyMatrix(r, y); // r = y
226         //omxDAXPY(-1.0, s, r); // r = r - s THAT IS r = y - (C x + D u)
227         omxResetAliasedMatrix(smallr);
228         
229         /* Filter S Here */
230         // N.B. if y is completely missing or completely present, leave S alone.
231         // Otherwise, filter S.
232         if(numRemovesSS < ny && numRemovesSS > 0) {
233                 //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S"); }
234                 if(OMX_DEBUG) { mxLog("Filtering S, R, C, r, K, and Y."); }
235                 omxRemoveRowsAndColumns(smallS, numRemovesSS, numRemovesSS, toRemoveSS, toRemoveSS);
236                 //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S (Filtered)"); }
237                 
238                 omxRemoveRowsAndColumns(smallR, numRemovesSS, numRemovesSS, toRemoveSS, toRemoveSS);
239                 
240                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C"); }
241                 omxRemoveRowsAndColumns(smallC, numRemovesSS, 0, toRemoveSS, toRemoveNoneLat);
242                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C (Filtered)"); }
243                 
244                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(r, "....State Space: r"); }
245                 omxRemoveRowsAndColumns(smallr, numRemovesSS, 0, toRemoveSS, toRemoveNoneOne);
246                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space: r (Filtered)"); }
247                 
248                 //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K"); }
249                 omxRemoveRowsAndColumns(smallK, numRemovesSS, 0, toRemoveSS, toRemoveNoneLat);
250                 //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K (Filtered)"); }
251                 
252                 omxRemoveRowsAndColumns(smallY, numRemovesSS, 0, toRemoveSS, toRemoveNoneLat);
253         }
254         if(numRemovesSS == ny) {
255                 if(OMX_DEBUG_ALGEBRA) { mxLog("Completely missing row of data found."); }
256                 if(OMX_DEBUG_ALGEBRA) { mxLog("Skipping much of Kalman Update."); }
257                 smallS->aliasedPtr = NULL;//delete alias from smallS
258                 return ;
259         }
260         
261         
262         /* S = C P C^T + R */
263         omxDSYMM(FALSE, 1.0, P, smallC, 0.0, smallY); // Y = C P
264         //omxCopyMatrix(S, smallR); // S = R
265         memcpy(smallS->data, smallR->data, smallR->rows * smallR->cols * sizeof(double)); // Less safe omxCopyMatrix that keeps smallS aliased to S.
266         omxDGEMM(FALSE, TRUE, 1.0, smallY, smallC, 1.0, smallS); // S = Y C^T + S THAT IS C P C^T + R
267         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S = C P C^T + R"); }
268         
269         
270         /* Now compute the Kalman Gain and update the error covariance matrix */
271         /* S = S^-1 */
272         omxDPOTRF(smallS, &info); // S replaced by the lower triangular matrix of the Cholesky factorization
273         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Cholesky of S"); }
274         covInfo->data[0] = (double) info;
275         for(int i = 0; i < smallS->cols; i++) {
276                 *Det->data += log(fabs(omxMatrixElement(smallS, i, i)));
277         }
278         //det *= 2.0; //sum( log( abs( diag( chol(S) ) ) ) )*2
279         omxDPOTRI(smallS, &info); // S = S^-1 via Cholesky factorization
280         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Inverse of S"); }
281         // If Cholesky of exp cov failed (i.e. non-positive def), Populate 1,1 element of smallS (inverse of exp cov) with NA_REAL
282         if(covInfo->data[0] > 0) {
283                 omxSetMatrixElement(smallS, 0, 0, NA_REAL);
284         }
285         
286         /* K = P C^T S^-1 */
287         /* Computed as K^T = S^-1 C P */
288         omxDSYMM(TRUE, 1.0, smallS, smallY, 0.0, smallK); // K = Y^T S THAT IS K = P C^T S^-1
289         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K^T = S^-1 C P"); }
290         
291         /* x = x + K r */
292         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space Check Residual: r"); }
293         omxDGEMV(TRUE, 1.0, smallK, smallr, 1.0, x); // x = K r + x
294         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x = K r + x"); }
295         
296         /* P = (I - K C) P */
297         /* P = P - K C P */
298         omxDGEMM(TRUE, FALSE, -1.0, smallK, smallY, 1.0, P); // P = -K Y + P THAT IS P = P - K C P
299         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = P - K C P"); }
300         
301         smallS->aliasedPtr = NULL;//delete alias from smallS
302         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Inverse of S"); }
303         
304         
305         /*m2ll = y^T S y */ // n.b. y originally is the data row but becomes the data residual!
306         //omxDSYMV(1.0, S, y, 0.0, s); // s = S y
307         //m2ll = omxDDOT(y, s); // m2ll = y s THAT IS y^T S y
308         //m2ll += det; // m2ll = m2ll + det THAT IS m2ll = log(det(S)) + y^T S y
309         // Note: this leaves off the S->cols * log(2*pi) THAT IS k*log(2*pi)
310 }
311
312
313
314 void omxInitStateSpaceExpectation(omxExpectation* ox) {
315         
316         SEXP rObj = ox->rObj;
317         if(OMX_DEBUG) { mxLog("Initializing State Space Expectation."); }
318                 
319         int nx, ny, nu;
320         
321         //SEXP slotValue;   //Used by PPML
322         
323         /* Create and fill expectation */
324         omxStateSpaceExpectation *SSMexp = (omxStateSpaceExpectation*) R_alloc(1, sizeof(omxStateSpaceExpectation));
325         omxState* currentState = ox->currentState;
326         
327         /* Set Expectation Calls and Structures */
328         ox->computeFun = omxCallStateSpaceExpectation;
329         ox->destructFun = omxDestroyStateSpaceExpectation;
330         ox->componentFun = omxGetStateSpaceExpectationComponent;
331         ox->mutateFun = omxSetStateSpaceExpectationComponent;
332         ox->populateAttrFun = omxPopulateSSMAttributes;
333         ox->argStruct = (void*) SSMexp;
334         
335         /* Set up expectation structures */
336         if(OMX_DEBUG) { mxLog("Initializing State Space Meta Data for expectation."); }
337         
338         if(OMX_DEBUG) { mxLog("Processing A."); }
339         SSMexp->A = omxNewMatrixFromSlot(rObj, currentState, "A");
340         
341         if(OMX_DEBUG) { mxLog("Processing B."); }
342         SSMexp->B = omxNewMatrixFromSlot(rObj, currentState, "B");
343         
344         if(OMX_DEBUG) { mxLog("Processing C."); }
345         SSMexp->C = omxNewMatrixFromSlot(rObj, currentState, "C");
346         
347         if(OMX_DEBUG) { mxLog("Processing D."); }
348         SSMexp->D = omxNewMatrixFromSlot(rObj, currentState, "D");
349         
350         if(OMX_DEBUG) { mxLog("Processing Q."); }
351         SSMexp->Q = omxNewMatrixFromSlot(rObj, currentState, "Q");
352         
353         if(OMX_DEBUG) { mxLog("Processing R."); }
354         SSMexp->R = omxNewMatrixFromSlot(rObj, currentState, "R");
355         
356         if(OMX_DEBUG) { mxLog("Processing initial x."); }
357         SSMexp->x0 = omxNewMatrixFromSlot(rObj, currentState, "x0");
358         
359         if(OMX_DEBUG) { mxLog("Processing initial P."); }
360         SSMexp->P0 = omxNewMatrixFromSlot(rObj, currentState, "P0");
361         
362         if(OMX_DEBUG) { mxLog("Processing u."); }
363         SSMexp->u = omxNewMatrixFromSlot(rObj, currentState, "u");
364
365         
366         
367         /* Initialize the place holder matrices used in calculations */
368         nx = SSMexp->C->cols;
369         ny = SSMexp->C->rows;
370         nu = SSMexp->D->cols;
371         
372         if(OMX_DEBUG) { mxLog("Processing first data row for y."); }
373         SSMexp->y = omxInitMatrix(NULL, ny, 1, TRUE, currentState);
374         for(int i = 0; i < ny; i++) {
375                 omxSetMatrixElement(SSMexp->y, i, 0, omxDoubleDataElement(ox->data, 0, i));
376         }
377         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(SSMexp->y, "....State Space: y"); }
378         
379         // TODO Make x0 and P0 static (if possible) to save memory
380         // TODO Look into omxMatrix.c/h for a possible new matrix from omxMatrix function
381         if(OMX_DEBUG) { mxLog("Generating static internals for resetting initial values."); }
382         SSMexp->x =     omxInitMatrix(NULL, nx, 1, TRUE, currentState);
383         SSMexp->P =     omxInitMatrix(NULL, nx, nx, TRUE, currentState);
384         omxCopyMatrix(SSMexp->x, SSMexp->x0);
385         omxCopyMatrix(SSMexp->P, SSMexp->P0);
386         
387         if(OMX_DEBUG) { mxLog("Generating internals for computation."); }
388         
389         SSMexp->covInfo =       omxInitMatrix(NULL, 1, 1, TRUE, currentState);
390         SSMexp->det =   omxInitMatrix(NULL, 1, 1, TRUE, currentState);
391         SSMexp->r =     omxInitMatrix(NULL, ny, 1, TRUE, currentState);
392         SSMexp->s =     omxInitMatrix(NULL, ny, 1, TRUE, currentState);
393         SSMexp->z =     omxInitMatrix(NULL, nx, 1, TRUE, currentState);
394         SSMexp->K =     omxInitMatrix(NULL, ny, nx, TRUE, currentState); // Actually the tranpose of the Kalman gain
395         SSMexp->S =     omxInitMatrix(NULL, ny, ny, TRUE, currentState);
396         SSMexp->Y =     omxInitMatrix(NULL, ny, nx, TRUE, currentState);
397         SSMexp->Z =     omxInitMatrix(NULL, nx, nx, TRUE, currentState);
398         
399         SSMexp->cov =           omxInitMatrix(NULL, ny, ny, TRUE, currentState);
400         SSMexp->means =         omxInitMatrix(NULL, 1, ny, TRUE, currentState);
401         
402         /* Create alias matrices for missing data filtering */
403         SSMexp->smallC =        omxInitMatrix(NULL, ny, nx, TRUE, currentState);
404         SSMexp->smallD =        omxInitMatrix(NULL, ny, nu, TRUE, currentState);
405         SSMexp->smallR =        omxInitMatrix(NULL, ny, ny, TRUE, currentState);
406         SSMexp->smallr =        omxInitMatrix(NULL, ny, 1, TRUE, currentState);
407         SSMexp->smallK =        omxInitMatrix(NULL, ny, nx, TRUE, currentState);
408         SSMexp->smallS =        omxInitMatrix(NULL, ny, ny, TRUE, currentState);
409         SSMexp->smallY =        omxInitMatrix(NULL, ny, nx, TRUE, currentState);
410         
411         /* Alias the small matrices so their alias pointers refer to the memory location of their respective matrices */
412         omxAliasMatrix(SSMexp->smallC, SSMexp->C);
413         omxAliasMatrix(SSMexp->smallD, SSMexp->D);
414         omxAliasMatrix(SSMexp->smallR, SSMexp->R);
415         omxAliasMatrix(SSMexp->smallr, SSMexp->r);
416         omxAliasMatrix(SSMexp->smallK, SSMexp->K);
417         omxAliasMatrix(SSMexp->smallS, SSMexp->S);
418         omxAliasMatrix(SSMexp->smallY, SSMexp->Y);
419
420 }
421
422
423 omxMatrix* omxGetStateSpaceExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component) {
424         omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);
425         omxMatrix* retval = NULL;
426
427         if(!strncmp("cov", component, 3)) {
428                 retval = ose->cov;
429         } else if(!strncmp("mean", component, 4)) {
430                 retval = ose->means;
431         } else if(!strncmp("pvec", component, 4)) {
432                 // Once implemented, change compute function and return pvec
433         } else if(!strncmp("inverse", component, 7)) {
434                 retval = ose->smallS;
435         } else if(!strncmp("determinant", component, 11)) {
436                 retval = ose->det;
437         } else if(!strncmp("r", component, 1)) {
438                 retval = ose->r;
439         } else if(OMXSTREQ("covInfo", component)) {
440                 retval = ose->covInfo;
441         }
442         
443         return retval;
444 }
445
446 void omxSetStateSpaceExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component, omxMatrix* om) {
447         omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);
448         
449         if(!strcmp("y", component)) {
450                 for(int i = 0; i < ose->y->rows; i++) {
451                         omxSetMatrixElement(ose->y, i, 0, omxVectorElement(om, i));
452                 }
453                 //ose->y = om;
454         }
455         if(!strcmp("Reset", component)) {
456                 omxCopyMatrix(ose->x, ose->x0);
457                 omxCopyMatrix(ose->P, ose->P0);
458         }
459 }
460
461
462