Add check for positive definite expected covariance matrix for state space expectations.
[openmx:openmx.git] / src / omxStateSpaceExpectation.cpp
1 /*
2  *  Copyright 2007-2013 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 *) {
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         /* We allocated 'em, so we destroy 'em. */
64         omxFreeMatrixData(argStruct->r);
65         omxFreeMatrixData(argStruct->s);
66         omxFreeMatrixData(argStruct->z);
67         //omxFreeMatrixData(argStruct->u); // This is data, destroy it?
68         //omxFreeMatrixData(argStruct->x); // This is latent data, destroy it?
69         omxFreeMatrixData(argStruct->y); // This is data, destroy it?
70         omxFreeMatrixData(argStruct->K); // This is the Kalman gain, destroy it?
71         //omxFreeMatrixData(argStruct->P); // This is latent cov, destroy it?
72         omxFreeMatrixData(argStruct->S); // This is data error cov, destroy it?
73         omxFreeMatrixData(argStruct->Y);
74         omxFreeMatrixData(argStruct->Z);
75 }
76
77
78 void omxPopulateSSMAttributes(omxExpectation *ox, SEXP algebra) {
79     if(OMX_DEBUG) { mxLog("Populating State Space Attributes.  Currently this does very little!"); }
80         
81 }
82
83
84
85
86 void omxKalmanPredict(omxStateSpaceExpectation* ose) {
87         if(OMX_DEBUG) { mxLog("Kalman Predict Called."); }
88         /* Creat local copies of State Space Matrices */
89         omxMatrix* A = ose->A;
90         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(A, "....State Space: A"); }
91         omxMatrix* B = ose->B;
92         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(B, "....State Space: B"); }
93         //omxMatrix* C = ose->C;
94         //omxMatrix* D = ose->D;
95         omxMatrix* Q = ose->Q;
96         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Q, "....State Space: Q"); }
97         //omxMatrix* R = ose->R;
98         //omxMatrix* r = ose->r;
99         //omxMatrix* s = ose->s;
100         omxMatrix* u = ose->u;
101         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(u, "....State Space: u"); }
102         omxMatrix* x = ose->x;
103         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x"); }
104         //omxMatrix* y = ose->y;
105         omxMatrix* z = ose->z;
106         //omxMatrix* K = ose->K;
107         omxMatrix* P = ose->P;
108         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P"); }
109         //omxMatrix* S = ose->S;
110         //omxMatrix* Y = ose->Y;
111         omxMatrix* Z = ose->Z;
112
113         /* x = A x + B u */
114         omxDGEMV(FALSE, 1.0, A, x, 0.0, z); // x = A x
115         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(z, "....State Space: z = A x"); }
116         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(A, "....State Space: A"); }
117         omxDGEMV(FALSE, 1.0, B, u, 1.0, z); // x = B u + x THAT IS x = A x + B u
118         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(z, "....State Space: z = A x + B u"); }
119         omxCopyMatrix(x, z); // x = z THAT IS x = A x + B u
120         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x = A x + B u"); }
121         
122         /* P = A P A^T + Q */
123         omxDSYMM(FALSE, 1.0, P, A, 0.0, Z); // Z = A P
124         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Z, "....State Space: Z = A P"); }
125         omxCopyMatrix(P, Q); // P = Q
126         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = Q"); }
127         omxDGEMM(FALSE, TRUE, 1.0, Z, A, 1.0, P); // P = Z A^T + P THAT IS P = A P A^T + Q
128         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = A P A^T + Q"); }
129 }
130
131
132 void omxKalmanUpdate(omxStateSpaceExpectation* ose) {
133         //TODO: Clean up this hack of function.
134         if(OMX_DEBUG) { mxLog("Kalman Update Called."); }
135         /* Creat local copies of State Space Matrices */
136         omxMatrix* C = ose->C;
137         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(C, "....State Space: C"); }
138         omxMatrix* D = ose->D;
139         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(D, "....State Space: D"); }
140         omxMatrix* R = ose->R; //unused
141         omxMatrix* r = ose->r;
142         omxMatrix* s = ose->s;
143         omxMatrix* u = ose->u;
144         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(u, "....State Space: u"); }
145         omxMatrix* x = ose->x;
146         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x"); }
147         omxMatrix* y = ose->y;
148         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(y, "....State Space: y"); }
149         omxMatrix* K = ose->K;
150         omxMatrix* P = ose->P;
151         omxMatrix* S = ose->S; //unused
152         omxMatrix* Y = ose->Y; //unused
153         omxMatrix* Cov = ose->cov;
154         omxMatrix* Means = ose->means;
155         omxMatrix* Det = ose->det;
156         *Det->data = 0.0; // the value pointed to by Det->data is assigned to be zero
157         omxMatrix* smallC = ose->smallC;
158         omxMatrix* smallD = ose->smallD;
159         omxMatrix* smallr = ose->smallr;
160         omxMatrix* smallR = ose->smallR;
161         omxMatrix* smallK = ose->smallK;
162         omxMatrix* smallS = ose->smallS;
163         omxMatrix* smallY = ose->smallY;
164         int ny = y->rows;
165         int nx = x->rows;
166         int toRemoveSS[ny];
167         int numRemovesSS = 0;
168         int toRemoveNoneLat[nx];
169         int toRemoveNoneOne[1];
170         memset(toRemoveNoneLat, 0, sizeof(int) * nx);
171         memset(toRemoveNoneOne, 0, sizeof(int) * 1);
172         
173         int info = 0; // Used for computing inverse for Kalman gain
174         
175         omxAliasMatrix(smallS, S); //re-alias every time expectation is called, at end delete the alias.
176         
177         /* Reset/Resample aliased matrices */
178         omxResetAliasedMatrix(smallC);
179         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C (Reset)"); }
180         omxResetAliasedMatrix(smallD);
181         omxResetAliasedMatrix(smallR);
182         omxResetAliasedMatrix(smallr);
183         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space: r (Reset)"); }
184         omxResetAliasedMatrix(smallK);
185         omxResetAliasedMatrix(smallS);
186         omxResetAliasedMatrix(smallY);
187         
188         /* r = r - C x - D u */
189         /* Alternatively, create just the expected value for the data row, x. */
190         omxDGEMV(FALSE, 1.0, smallC, x, 0.0, s); // s = C x
191         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(s, "....State Space: s = C x"); }
192         omxDGEMV(FALSE, 1.0, D, u, 1.0, s); // s = D u + s THAT IS s = C x + D u
193         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(s, "....State Space: s = C x + D u"); }
194         omxCopyMatrix(Means, s); // Means = s THAT IS Means = C x + D u
195         //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Means, "....State Space: Means"); }
196         omxTransposeMatrix(Means);
197         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Means, "....State Space: Means"); }
198         
199         //If entire data vector, y, is missing, then set residual, r, to zero.
200         //otherwise, compute residual.
201         memset(toRemoveSS, 0, sizeof(int) * ny);
202         for(int j = 0; j < y->rows; j++) {
203                 double dataValue = omxMatrixElement(y, j, 0);
204                 int dataValuefpclass = fpclassify(dataValue);
205                 if(dataValuefpclass == FP_NAN || dataValuefpclass == FP_INFINITE) {
206                         numRemovesSS++;
207                         toRemoveSS[j] = 1;
208                         omxSetMatrixElement(r, j, 0, 0.0);
209                 } else {
210                         omxSetMatrixElement(r, j, 0, (dataValue -  omxMatrixElement(s, j, 0)));
211                 }
212         }
213         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(r, "....State Space: Residual (Loop)"); }
214         /* Now compute the residual */
215         //omxCopyMatrix(r, y); // r = y
216         //omxDAXPY(-1.0, s, r); // r = r - s THAT IS r = y - (C x + D u)
217         omxResetAliasedMatrix(smallr);
218         
219         /* Filter S Here */
220         // N.B. if y is completely missing or completely present, leave S alone.
221         // Otherwise, filter S.
222         if(numRemovesSS < ny && numRemovesSS > 0) {
223                 //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S"); }
224                 if(OMX_DEBUG) { mxLog("Filtering S, R, C, r, K, and Y."); }
225                 omxRemoveRowsAndColumns(smallS, numRemovesSS, numRemovesSS, toRemoveSS, toRemoveSS);
226                 //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S (Filtered)"); }
227                 
228                 omxRemoveRowsAndColumns(smallR, numRemovesSS, numRemovesSS, toRemoveSS, toRemoveSS);
229                 
230                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C"); }
231                 omxRemoveRowsAndColumns(smallC, numRemovesSS, 0, toRemoveSS, toRemoveNoneLat);
232                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallC, "....State Space: C (Filtered)"); }
233                 
234                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(r, "....State Space: r"); }
235                 omxRemoveRowsAndColumns(smallr, numRemovesSS, 0, toRemoveSS, toRemoveNoneOne);
236                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space: r (Filtered)"); }
237                 
238                 //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K"); }
239                 omxRemoveRowsAndColumns(smallK, numRemovesSS, 0, toRemoveSS, toRemoveNoneLat);
240                 //if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K (Filtered)"); }
241                 
242                 omxRemoveRowsAndColumns(smallY, numRemovesSS, 0, toRemoveSS, toRemoveNoneLat);
243         }
244         if(numRemovesSS == ny) {
245                 if(OMX_DEBUG_ALGEBRA) { mxLog("Completely missing row of data found."); }
246                 if(OMX_DEBUG_ALGEBRA) { mxLog("Skipping much of Kalman Update."); }
247                 smallS->aliasedPtr = NULL;//delete alias from smallS
248                 return ;
249         }
250         
251         
252         /* S = C P C^T + R */
253         omxDSYMM(FALSE, 1.0, P, smallC, 0.0, smallY); // Y = C P
254         //omxCopyMatrix(S, smallR); // S = R
255         memcpy(smallS->data, smallR->data, smallR->rows * smallR->cols * sizeof(double)); // Less safe omxCopyMatrix that keeps smallS aliased to S.
256         omxDGEMM(FALSE, TRUE, 1.0, smallY, smallC, 1.0, smallS); // S = Y C^T + S THAT IS C P C^T + R
257         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: S = C P C^T + R"); }
258         
259         
260         /* Now compute the Kalman Gain and update the error covariance matrix */
261         /* S = S^-1 */
262         omxDPOTRF(smallS, &info); // S replaced by the lower triangular matrix of the Cholesky factorization
263         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Cholesky of S"); }
264         if(info > 0) {
265                 char *errstr = (char*) calloc(250, sizeof(char));
266                 sprintf(errstr, "Expected covariance matrix is non-positive-definite");
267                 strncat(errstr, ".\n", 3);
268                 omxRaiseErrorf(ose->currentState, errstr); // Raise error
269                 free(errstr);
270                 return;  // Leave output untouched
271         }
272         for(int i = 0; i < smallS->cols; i++) {
273                 *Det->data += log(fabs(omxMatrixElement(smallS, i, i)));
274         }
275         //det *= 2.0; //sum( log( abs( diag( chol(S) ) ) ) )*2
276         omxDPOTRI(smallS, &info); // S = S^-1 via Cholesky factorization
277         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Inverse of S"); }
278         
279         
280         /* K = P C^T S^-1 */
281         /* Computed as K^T = S^-1 C P */
282         omxDSYMM(TRUE, 1.0, smallS, smallY, 0.0, smallK); // K = Y^T S THAT IS K = P C^T S^-1
283         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallK, "....State Space: K^T = S^-1 C P"); }
284         
285         /* x = x + K r */
286         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallr, "....State Space Check Residual: r"); }
287         omxDGEMV(TRUE, 1.0, smallK, smallr, 1.0, x); // x = K r + x
288         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(x, "....State Space: x = K r + x"); }
289         
290         /* P = (I - K C) P */
291         /* P = P - K C P */
292         omxDGEMM(TRUE, FALSE, -1.0, smallK, smallY, 1.0, P); // P = -K Y + P THAT IS P = P - K C P
293         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(P, "....State Space: P = P - K C P"); }
294         
295         smallS->aliasedPtr = NULL;//delete alias from smallS
296         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(smallS, "....State Space: Inverse of S"); }
297         
298         
299         /*m2ll = y^T S y */ // n.b. y originally is the data row but becomes the data residual!
300         //omxDSYMV(1.0, S, y, 0.0, s); // s = S y
301         //m2ll = omxDDOT(y, s); // m2ll = y s THAT IS y^T S y
302         //m2ll += det; // m2ll = m2ll + det THAT IS m2ll = log(det(S)) + y^T S y
303         // Note: this leaves off the S->cols * log(2*pi) THAT IS k*log(2*pi)
304 }
305
306
307
308 void omxInitStateSpaceExpectation(omxExpectation* ox) {
309         
310         SEXP rObj = ox->rObj;
311         if(OMX_DEBUG) { mxLog("Initializing State Space Expectation."); }
312                 
313         int nx, ny, nu;
314         
315         //SEXP slotValue;   //Used by PPML
316         
317         /* Create and fill expectation */
318         omxStateSpaceExpectation *SSMexp = (omxStateSpaceExpectation*) R_alloc(1, sizeof(omxStateSpaceExpectation));
319         omxState* currentState = ox->currentState;
320         SSMexp->currentState = currentState; //??? not sure if this is the right way to do this.  The SSMexp->currentState should point to the same memory location as the ox->currentState.
321         
322         /* Set Expectation Calls and Structures */
323         ox->computeFun = omxCallStateSpaceExpectation;
324         ox->destructFun = omxDestroyStateSpaceExpectation;
325         ox->componentFun = omxGetStateSpaceExpectationComponent;
326         ox->mutateFun = omxSetStateSpaceExpectationComponent;
327         ox->populateAttrFun = omxPopulateSSMAttributes;
328         ox->argStruct = (void*) SSMexp;
329         
330         /* Set up expectation structures */
331         if(OMX_DEBUG) { mxLog("Initializing State Space Meta Data for expectation."); }
332         
333         if(OMX_DEBUG) { mxLog("Processing A."); }
334         SSMexp->A = omxNewMatrixFromSlot(rObj, currentState, "A");
335         
336         if(OMX_DEBUG) { mxLog("Processing B."); }
337         SSMexp->B = omxNewMatrixFromSlot(rObj, currentState, "B");
338         
339         if(OMX_DEBUG) { mxLog("Processing C."); }
340         SSMexp->C = omxNewMatrixFromSlot(rObj, currentState, "C");
341         
342         if(OMX_DEBUG) { mxLog("Processing D."); }
343         SSMexp->D = omxNewMatrixFromSlot(rObj, currentState, "D");
344         
345         if(OMX_DEBUG) { mxLog("Processing Q."); }
346         SSMexp->Q = omxNewMatrixFromSlot(rObj, currentState, "Q");
347         
348         if(OMX_DEBUG) { mxLog("Processing R."); }
349         SSMexp->R = omxNewMatrixFromSlot(rObj, currentState, "R");
350         
351         if(OMX_DEBUG) { mxLog("Processing initial x."); }
352         SSMexp->x0 = omxNewMatrixFromSlot(rObj, currentState, "x0");
353         
354         if(OMX_DEBUG) { mxLog("Processing initial P."); }
355         SSMexp->P0 = omxNewMatrixFromSlot(rObj, currentState, "P0");
356         
357         
358         /* Initialize the place holder matrices used in calculations */
359         nx = SSMexp->C->cols;
360         ny = SSMexp->C->rows;
361         nu = SSMexp->D->cols;
362         
363         if(OMX_DEBUG) { mxLog("Processing first data row for y."); }
364         SSMexp->y = omxInitMatrix(NULL, ny, 1, TRUE, currentState);
365         for(int i = 0; i < ny; i++) {
366                 omxSetMatrixElement(SSMexp->y, i, 0, omxDoubleDataElement(ox->data, 0, i));
367         }
368         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(SSMexp->y, "....State Space: y"); }
369         
370         // TODO Make x0 and P0 static (if possible) to save memory
371         // TODO Look into omxMatrix.c/h for a possible new matrix from omxMatrix function
372         if(OMX_DEBUG) { mxLog("Generating static internals for resetting initial values."); }
373         SSMexp->x =     omxInitMatrix(NULL, nx, 1, TRUE, currentState);
374         SSMexp->P =     omxInitMatrix(NULL, nx, nx, TRUE, currentState);
375         omxCopyMatrix(SSMexp->x, SSMexp->x0);
376         omxCopyMatrix(SSMexp->P, SSMexp->P0);
377         
378         if(OMX_DEBUG) { mxLog("Generating internals for computation."); }
379         
380         SSMexp->det =   omxInitMatrix(NULL, 1, 1, TRUE, currentState);
381         SSMexp->r =     omxInitMatrix(NULL, ny, 1, TRUE, currentState);
382         SSMexp->s =     omxInitMatrix(NULL, ny, 1, TRUE, currentState);
383         SSMexp->u =     omxInitMatrix(NULL, nu, 1, TRUE, currentState);
384         SSMexp->z =     omxInitMatrix(NULL, nx, 1, TRUE, currentState);
385         SSMexp->K =     omxInitMatrix(NULL, ny, nx, TRUE, currentState); // Actually the tranpose of the Kalman gain
386         SSMexp->S =     omxInitMatrix(NULL, ny, ny, TRUE, currentState);
387         SSMexp->Y =     omxInitMatrix(NULL, ny, nx, TRUE, currentState);
388         SSMexp->Z =     omxInitMatrix(NULL, nx, nx, TRUE, currentState);
389         
390         SSMexp->cov =           omxInitMatrix(NULL, ny, ny, TRUE, currentState);
391         SSMexp->means =         omxInitMatrix(NULL, 1, ny, TRUE, currentState);
392         
393         /* Create alias matrices for missing data filtering */
394         SSMexp->smallC =        omxInitMatrix(NULL, ny, nx, TRUE, currentState);
395         SSMexp->smallD =        omxInitMatrix(NULL, ny, nu, TRUE, currentState);
396         SSMexp->smallR =        omxInitMatrix(NULL, ny, ny, TRUE, currentState);
397         SSMexp->smallr =        omxInitMatrix(NULL, ny, 1, TRUE, currentState);
398         SSMexp->smallK =        omxInitMatrix(NULL, ny, nx, TRUE, currentState);
399         SSMexp->smallS =        omxInitMatrix(NULL, ny, ny, TRUE, currentState);
400         SSMexp->smallY =        omxInitMatrix(NULL, ny, nx, TRUE, currentState);
401         
402         /* Alias the small matrices so their alias pointers refer to the memory location of their respective matrices */
403         omxAliasMatrix(SSMexp->smallC, SSMexp->C);
404         omxAliasMatrix(SSMexp->smallD, SSMexp->D);
405         omxAliasMatrix(SSMexp->smallR, SSMexp->R);
406         omxAliasMatrix(SSMexp->smallr, SSMexp->r);
407         omxAliasMatrix(SSMexp->smallK, SSMexp->K);
408         omxAliasMatrix(SSMexp->smallS, SSMexp->S);
409         omxAliasMatrix(SSMexp->smallY, SSMexp->Y);
410
411 }
412
413
414 omxMatrix* omxGetStateSpaceExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component) {
415         omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);
416         omxMatrix* retval = NULL;
417
418         if(!strncmp("cov", component, 3)) {
419                 retval = ose->cov;
420         } else if(!strncmp("mean", component, 4)) {
421                 retval = ose->means;
422         } else if(!strncmp("pvec", component, 4)) {
423                 // Once implemented, change compute function and return pvec
424         } else if(!strncmp("inverse", component, 7)) {
425                 retval = ose->smallS;
426         } else if(!strncmp("determinant", component, 11)) {
427                 retval = ose->det;
428         } else if(!strncmp("r", component, 1)) {
429                 retval = ose->r;
430         }
431         
432         if(OMX_DEBUG) { mxLog("Returning %p.", retval); }
433
434         return retval;
435 }
436
437 void omxSetStateSpaceExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component, omxMatrix* om) {
438         omxStateSpaceExpectation* ose = (omxStateSpaceExpectation*)(ox->argStruct);
439         
440         if(!strcmp("y", component)) {
441                 for(int i = 0; i < ose->y->rows; i++) {
442                         omxSetMatrixElement(ose->y, i, 0, omxVectorElement(om, i));
443                 }
444                 //ose->y = om;
445         }
446         if(!strcmp("Reset", component)) {
447                 omxCopyMatrix(ose->x, ose->x0);
448                 omxCopyMatrix(ose->P, ose->P0);
449         }
450 }
451
452
453