Roll BA81 fit function into FitFunctionML
[openmx:openmx.git] / src / omxMLFitFunction.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 #include <R.h>
18 #include <Rinternals.h>
19 #include <Rdefines.h>
20 #include <R_ext/Rdynload.h>
21 #include <R_ext/BLAS.h>
22 #include <R_ext/Lapack.h>
23 #include "omxAlgebraFunctions.h"
24 #include "omxExpectation.h"
25 #include "omxMLFitFunction.h"
26 #include "omxFIMLFitFunction.h"
27 #include "omxRAMExpectation.h"
28
29 void omxDestroyMLFitFunction(omxFitFunction *oo) {
30
31         if(OMX_DEBUG) {mxLog("Freeing ML Fit Function.");}
32         omxMLFitFunction* omlo = ((omxMLFitFunction*)oo->argStruct);
33
34         if(omlo->localCov != NULL)      omxFreeMatrixData(omlo->localCov);
35         if(omlo->localProd != NULL)     omxFreeMatrixData(omlo->localProd);
36         if(omlo->P != NULL)                     omxFreeMatrixData(omlo->P);
37         if(omlo->C != NULL)                     omxFreeMatrixData(omlo->C);
38         if(omlo->I != NULL)                     omxFreeMatrixData(omlo->I);
39 }
40
41 static void calcExtraLikelihoods(omxFitFunction *oo, double *saturated_out, double *independence_out)
42 {
43         omxMLFitFunction *state = (omxMLFitFunction*) oo->argStruct;
44         double det = 0.0;
45         omxMatrix* cov = state->observedCov;
46         int ncols = state->observedCov->cols;
47     
48         *saturated_out = (state->logDetObserved + ncols) * (state->n - 1);
49
50         // Independence model assumes all-zero manifest covariances.
51         // (det(expected) + tr(observed * expected^-1)) * (n - 1);
52         // expected is the diagonal of the observed.  Inverse expected is 1/each diagonal value.
53         // Therefore the diagonal elements of observed * expected^-1 are each 1.
54         // So the trace of the matrix is the same as the number of columns.
55         // The determinant of a diagonal matrix is the product of the diagonal elements.
56         // Since these are the same in the expected as in the observed, we can get 'em direct.
57
58         for(int i = 0; i < ncols; i++) {
59                 // We sum logs instead of logging the product.
60                 det += log(omxMatrixElement(cov, i, i));
61         }
62         if(OMX_DEBUG) { mxLog("det: %f, tr: %f, n= %d, total:%f", det, ncols, state->n, (ncols + det) * (state->n - 1)); }
63         if(OMX_DEBUG) { omxPrint(cov, "Observed:"); }
64
65         *independence_out = (ncols + det) * (state->n - 1);
66 }
67
68 static void addOutput(omxFitFunction *oo, MxRList *out)
69 {
70         // DEPRECATED, use omxPopulateMLAttributes
71         double saturated_out;
72         double independence_out;
73         calcExtraLikelihoods(oo, &saturated_out, &independence_out);
74         out->push_back(std::make_pair(mkChar("SaturatedLikelihood"),
75                                       ScalarReal(saturated_out)));
76         out->push_back(std::make_pair(mkChar("IndependenceLikelihood"),
77                                       ScalarReal(independence_out)));
78 }
79
80 static void omxCallMLFitFunction(omxFitFunction *oo, int want, FitContext *) {
81
82         if (want & (FF_COMPUTE_PREOPTIMIZE | FF_COMPUTE_POSTOPTIMIZE)) return;
83
84         if(OMX_DEBUG) { mxLog("Beginning ML Evaluation.");}
85         // Requires: Data, means, covariances.
86
87         double sum = 0.0, det = 0.0;
88         char u = 'U';
89         char r = 'R';
90         int info = 0;
91         double oned = 1.0;
92         double zerod = 0.0;
93         double minusoned = -1.0;
94         int onei = 1;
95         double fmean = 0.0;
96
97         omxMatrix *scov, *smeans, *cov, *means, *localCov, *localProd, *P, *C;
98
99         omxMLFitFunction *omo = ((omxMLFitFunction*)oo->argStruct);
100         
101     /* Locals for readability.  Compiler should cut through this. */
102         scov            = omo->observedCov;
103         smeans          = omo->observedMeans;
104         cov                     = omo->expectedCov;
105         means           = omo->expectedMeans;
106         localCov        = omo->localCov;
107         localProd       = omo->localProd;
108         P                       = omo->P;
109         C                       = omo->C;
110         double n        = omo->n;
111         double Q        = omo->logDetObserved;
112         omxExpectation* expectation = oo->expectation;
113
114     /* Recompute and recopy */
115         omxExpectationCompute(expectation, NULL);
116
117         omxCopyMatrix(localCov, cov);                           // But expected cov is destroyed in inversion
118
119         if(OMX_DEBUG_ALGEBRA) {
120                 omxPrint(scov, "Observed Covariance is");
121                 omxPrint(localCov, "Implied Covariance Is");
122                 omxPrint(cov, "Original Covariance Is");
123         }
124
125         /* Calculate |expected| */
126
127 //      F77_CALL(dgetrf)(&(localCov->cols), &(localCov->rows), localCov->data, &(localCov->cols), ipiv, &info);
128         F77_CALL(dpotrf)(&u, &(localCov->cols), localCov->data, &(localCov->cols), &info);
129
130         if(OMX_DEBUG_ALGEBRA) { mxLog("Info on LU Decomp: %d", info);}
131         if(info > 0) {
132                 omxRaiseErrorf(oo->matrix->currentState,
133                                "Expected covariance matrix is non-positive-definite after %d evaluations",
134                                oo->matrix->currentState->computeCount);
135                 return;
136         }
137
138         //det = log(det)        // TVO: changed multiplication of det to first log and the summing up; this line should just set det to zero.
139         for(info = 0; info < localCov->cols; info++) {          // |cov| is the square of the product of the diagonal elements of U from the LU factorization.
140                 det += log(fabs(localCov->data[info+localCov->rows*info])); // TVO: changed * to + and added fabs command
141         }
142         det *= 2.0;             // TVO: instead of det *= det;
143
144         if(OMX_DEBUG_ALGEBRA) { mxLog("Determinant of Expected Cov: %f", exp(det)); }
145         // TVO: removed det = log(fabs(det))
146         if(OMX_DEBUG_ALGEBRA) { mxLog("Log of Determinant of Expected Cov: %f", det); }
147
148         /* Calculate Expected^(-1) */
149 //      F77_CALL(dgetri)(&(localCov->rows), localCov->data, &(localCov->cols), ipiv, work, lwork, &info);
150         F77_CALL(dpotri)(&u, &(localCov->rows), localCov->data, &(localCov->cols), &info);
151         if(OMX_DEBUG_ALGEBRA) { mxLog("Info on Invert: %d", info); }
152
153         if(OMX_DEBUG_ALGEBRA) {omxPrint(cov, "Expected Covariance Matrix:");}
154         if(OMX_DEBUG_ALGEBRA) {omxPrint(localCov, "Inverted Matrix:");}
155
156         /* Calculate C = Observed * expected^(-1) */
157
158         if(OMX_DEBUG_ALGEBRA) {mxLog("Call is: DSYMM(%d, %d, %f, %0x, %d, %0x, %d, %f, %0x, %d)",
159                                         (scov->rows), (localCov->cols), oned, scov->data, (localCov->leading),
160                                         localCov->data, (localCov->leading), zerod, localProd->data, (localProd->leading));}
161
162
163         // Stop gcc from issuing a warning
164         int majority = *(scov->majority) == 'n' ? scov->rows : scov->cols;
165
166         /*  TODO:  Make sure leading edges are being appropriately calculated, and sub them back into this */
167         F77_CALL(dsymm)(&r, &u, &(localCov->rows), &(scov->cols),
168                                         &oned, localCov->data, &(majority),
169                                         scov->data, &(majority),
170                                         &zerod, localProd->data, &(localProd->leading));
171
172     /* And get the trace of the result */
173
174         for(info = 0; info < localCov->cols; info++) {
175                 sum += localProd->data[info*localCov->cols + info];
176         }
177
178 //      for(info = 0; info < (localCov->cols * localCov->rows); info++) {
179 //              sum += localCov->data[info] * scov->data[info];
180 //      }
181
182         if(OMX_DEBUG_ALGEBRA) {omxPrint(scov, "Observed Covariance Matrix:");}
183         if(OMX_DEBUG_ALGEBRA) {omxPrint(localCov, "Inverse Matrix:");}
184         if(OMX_DEBUG_ALGEBRA) {omxPrint(localProd, "Product Matrix:");}
185
186         if(means != NULL) {
187                 if(OMX_DEBUG_ALGEBRA) { mxLog("Means Likelihood Calculation"); }
188                 omxRecompute(means);
189                 omxCopyMatrix(P, means);
190                 // P = means - smeans
191                 if(OMX_DEBUG_ALGEBRA) {omxPrint(means, "means");}
192                 if(OMX_DEBUG_ALGEBRA) {omxPrint(smeans, "smeans");}
193                 F77_CALL(daxpy)(&(smeans->cols), &minusoned, smeans->data, &onei, P->data, &onei);
194                 if(OMX_DEBUG_ALGEBRA) {omxPrint(P, "means - smeans");}
195                 // C = P * Cov
196                 F77_CALL(dsymv)(&u, &(localCov->rows), &oned, localCov->data, &(localCov->leading), P->data, &onei, &zerod, C->data, &onei);
197                 // P = C * P'
198                 fmean = F77_CALL(ddot)(&(C->cols), P->data, &onei, C->data, &onei);
199
200                 if(OMX_DEBUG_ALGEBRA) { mxLog("Mean contribution to likelihood is %f per row.", fmean); }
201                 if(fmean < 0.0) fmean = 0.0;
202         }
203
204         oo->matrix->data[0] = (sum + det) * (n - 1) + fmean * (n);
205
206         if(OMX_DEBUG) { mxLog("MLFitFunction value comes to: %f (Chisq: %f).", oo->matrix->data[0], (sum + det) - Q - cov->cols); }
207
208 }
209
210 void omxPopulateMLAttributes(omxFitFunction *oo, SEXP algebra) {
211     if(OMX_DEBUG) { mxLog("Populating ML Attributes."); }
212
213         omxMLFitFunction *argStruct = ((omxMLFitFunction*)oo->argStruct);
214         omxMatrix *expCovInt = argStruct->expectedCov;                  // Expected covariance
215         omxMatrix *expMeanInt = argStruct->expectedMeans;                       // Expected means
216
217         SEXP expCovExt, expMeanExt, gradients;
218         PROTECT(expCovExt = allocMatrix(REALSXP, expCovInt->rows, expCovInt->cols));
219         for(int row = 0; row < expCovInt->rows; row++)
220                 for(int col = 0; col < expCovInt->cols; col++)
221                         REAL(expCovExt)[col * expCovInt->rows + row] =
222                                 omxMatrixElement(expCovInt, row, col);
223
224         if (expMeanInt != NULL) {
225                 PROTECT(expMeanExt = allocMatrix(REALSXP, expMeanInt->rows, expMeanInt->cols));
226                 for(int row = 0; row < expMeanInt->rows; row++)
227                         for(int col = 0; col < expMeanInt->cols; col++)
228                                 REAL(expMeanExt)[col * expMeanInt->rows + row] =
229                                         omxMatrixElement(expMeanInt, row, col);
230         } else {
231                 PROTECT(expMeanExt = allocMatrix(REALSXP, 0, 0));               
232         }   
233
234         if (0) {
235                 // TODO, fix for new gradient internal API
236         } else {
237                 PROTECT(gradients = allocMatrix(REALSXP, 0, 0));
238         }
239     
240         setAttrib(algebra, install("expCov"), expCovExt);
241         setAttrib(algebra, install("expMean"), expMeanExt);
242         setAttrib(algebra, install("gradients"), gradients);
243         
244         double saturated_out;
245         double independence_out;
246         calcExtraLikelihoods(oo, &saturated_out, &independence_out);
247         setAttrib(algebra, install("SaturatedLikelihood"), ScalarReal(saturated_out));
248         setAttrib(algebra, install("IndependenceLikelihood"), ScalarReal(independence_out));
249
250         UNPROTECT(3);
251 }
252
253 void omxSetMLFitFunctionCalls(omxFitFunction* oo) {
254         
255         /* Set FitFunction Calls to ML FitFunction Calls */
256         oo->computeFun = omxCallMLFitFunction;
257         oo->destructFun = omxDestroyMLFitFunction;
258         oo->addOutput = addOutput;
259         oo->populateAttrFun = omxPopulateMLAttributes;
260 }
261
262
263 void omxInitMLFitFunction(omxFitFunction* oo)
264 {
265         omxExpectation *expectation = oo->expectation;
266         if (strcmp(expectation->expType, "MxExpectationBA81")==0) {
267                 omxInitFitFunctionBA81(oo);
268                 return;
269         }
270
271         if(OMX_DEBUG) { mxLog("Initializing ML fit function."); }
272
273         int info = 0;
274         double det = 1.0;
275         char u = 'U';
276         
277         /* Read and set expectation */
278         omxSetMLFitFunctionCalls(oo);
279
280         if (!oo->expectation) { error("%s requires an expectation", oo->fitType); }
281
282         omxData* dataMat = oo->expectation->data;
283
284         if(!(dataMat == NULL) && strncmp(omxDataType(dataMat), "cov", 3) != 0 && strncmp(omxDataType(dataMat), "cor", 3) != 0) {
285                 if(strncmp(omxDataType(dataMat), "raw", 3) == 0) {
286                         if(OMX_DEBUG) { mxLog("Raw Data: Converting to FIML."); }
287                         omxInitFIMLFitFunction(oo);
288                         return;
289                 }
290                 char *errstr = (char*) calloc(250, sizeof(char));
291                 sprintf(errstr, "ML FitFunction unable to handle data type %s.\n", omxDataType(dataMat));
292                 omxRaiseError(oo->matrix->currentState, -1, errstr);
293                 free(errstr);
294                 if(OMX_DEBUG) { mxLog("ML FitFunction unable to handle data type %s.  Aborting.", omxDataType(dataMat)); }
295                 return;
296         }
297
298         omxMLFitFunction *newObj = (omxMLFitFunction*) R_alloc(1, sizeof(omxMLFitFunction));
299         oo->argStruct = (void*)newObj;
300
301         if(OMX_DEBUG) { mxLog("Processing Observed Covariance."); }
302         newObj->observedCov = omxDataMatrix(dataMat, NULL);
303         if(OMX_DEBUG) { mxLog("Processing Observed Means."); }
304         newObj->observedMeans = omxDataMeans(dataMat, NULL, NULL);
305         if(OMX_DEBUG && newObj->observedMeans == NULL) { mxLog("ML: No Observed Means."); }
306         if(OMX_DEBUG) { mxLog("Processing n."); }
307         newObj->n = omxDataNumObs(dataMat);
308
309         newObj->expectedCov = omxGetExpectationComponent(oo->expectation, oo, "cov");
310         newObj->expectedMeans = omxGetExpectationComponent(oo->expectation, oo, "means");
311
312         if(newObj->expectedCov == NULL) {
313                 omxRaiseError(oo->matrix->currentState, OMX_DEVELOPER_ERROR,
314                         "Developer Error in ML-based fit function object: ML's expectation must specify a model-implied covariance matrix.\nIf you are not developing a new expectation type, you should probably post this to the OpenMx forums.");
315                 return;
316         }
317
318         // Error Checking: Observed/Expected means must agree.  
319         // ^ is XOR: true when one is false and the other is not.
320         if((newObj->expectedMeans == NULL) ^ (newObj->observedMeans == NULL)) {
321                 if(newObj->expectedMeans != NULL) {
322                         omxRaiseError(oo->matrix->currentState, OMX_ERROR,
323                                 "Observed means not detected, but an expected means matrix was specified.\n  If you provide observed means, you must specify a model for the means.\n");
324                         return;
325                 } else {
326                         omxRaiseError(oo->matrix->currentState, OMX_ERROR,
327                                 "Observed means were provided, but an expected means matrix was not specified.\n  If you  wish to model the means, you must provide observed means.\n");
328                         return;         
329                 }
330         }
331
332         /* Temporary storage for calculation */
333         int rows = newObj->observedCov->rows;
334         int cols = newObj->observedCov->cols;
335         newObj->localCov = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
336         newObj->localProd = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
337         newObj->P = omxInitMatrix(NULL, 1, cols, TRUE, oo->matrix->currentState);
338         newObj->C = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
339         newObj->I = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
340
341         for(int i = 0; i < rows; i++) omxSetMatrixElement(newObj->I, i, i, 1.0);
342
343         omxCopyMatrix(newObj->localCov, newObj->observedCov);
344
345         newObj->lwork = newObj->expectedCov->rows;
346         newObj->work = (double*)R_alloc(newObj->lwork, sizeof(double));
347
348         // TODO: Determine where the saturated model computation should go.
349
350         F77_CALL(dpotrf)(&u, &(newObj->localCov->cols), newObj->localCov->data, &(newObj->localCov->cols), &info);
351
352         if(OMX_DEBUG) { mxLog("Info on LU Decomp: %d", info); }
353         if(info != 0) {
354                 char *errstr = (char*) calloc(250, sizeof(char));
355                 sprintf(errstr, "Observed Covariance Matrix is non-positive-definite.\n");
356                 omxRaiseError(oo->matrix->currentState, -1, errstr);
357                 free(errstr);
358                 return;
359         }
360         for(info = 0; info < newObj->localCov->cols; info++) {
361                 det *= omxMatrixElement(newObj->localCov, info, info);
362         }
363         det *= det;                                     // Product of squares.
364
365         if(OMX_DEBUG) { mxLog("Determinant of Observed Cov: %f", det); }
366         newObj->logDetObserved = log(det);
367         if(OMX_DEBUG) { mxLog("Log Determinant of Observed Cov: %f", newObj->logDetObserved); }
368
369         omxCopyMatrix(newObj->localCov, newObj->expectedCov);
370 }
371
372 void omxSetMLFitFunctionGradient(omxFitFunction* oo, void (*derivativeFun)(omxFitFunction*, double*)) {
373     if(strncmp("omxMLFitFunction", oo->fitType, 16)) {
374         char errorstr[250];
375         sprintf(errorstr, "PROGRAMMER ERROR: Using vanilla-ML gradient with FitFunction of type %s", oo->fitType);
376         omxRaiseError(oo->matrix->currentState, -2, errorstr);
377         return;
378     }
379     
380     if(derivativeFun == NULL) {
381         char errorstr[250];
382         sprintf(errorstr, "Programmer error: ML gradient given NULL gradient function.");
383         omxRaiseError(oo->matrix->currentState, -2, errorstr);
384         return;
385     }
386     
387     //oo->gradientFun = derivativeFun; TODO
388 }
389
390 void omxSetMLFitFunctionGradientComponents(omxFitFunction* oo, void (*derivativeFun)(omxFitFunction*, omxMatrix**, omxMatrix**, int*)) {
391     if(OMX_DEBUG) { mxLog("Setting up gradient component function for ML FitFunction."); }
392     if(!strncmp("omxFIMLFitFunction", oo->fitType, 16)) {
393         if(OMX_DEBUG) { mxLog("FIML FitFunction gradients not yet implemented. Skipping."); }
394         return; // ERROR:NYI.
395     }
396     
397     if(derivativeFun == NULL) {
398         char errorstr[250];
399         sprintf(errorstr, "Programmer error: ML gradient components given NULL gradient function.");
400         omxRaiseError(oo->matrix->currentState, -2, errorstr);
401         return;
402     }
403     
404     omxMLFitFunction *omo = ((omxMLFitFunction*) oo->argStruct);
405     int rows = omo->observedCov->rows;
406     int cols = omo->observedCov->cols;
407     size_t nFreeVars = oo->freeVarGroup->vars.size();
408             
409     omo->derivativeFun = derivativeFun;
410     omo->X  = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
411     omo->Y  = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
412     omo->Ms = omxInitMatrix(NULL, 1, cols, TRUE, oo->matrix->currentState);
413     omo->Mu = omxInitMatrix(NULL, 1, cols, TRUE, oo->matrix->currentState);
414     omo->dSigma = (omxMatrix**) R_alloc(nFreeVars, sizeof(omxMatrix*));
415     omo->dMu = (omxMatrix**) R_alloc(nFreeVars, sizeof(omxMatrix*));
416     for(size_t i = 0; i < nFreeVars; i++) {
417         omo->dSigma[i] = omxInitMatrix(NULL, rows, cols, TRUE, oo->matrix->currentState);
418         omo->dMu[i] = omxInitMatrix(NULL, rows, 1, TRUE, oo->matrix->currentState);
419     }
420     //oo->gradientFun = omxCalculateMLGradient; TODO
421 }
422
423 void omxCalculateMLGradient(omxFitFunction* oo, double* gradient) {
424
425     if(OMX_DEBUG) { mxLog("Beginning ML Gradient Calculation."); }
426     // mxLog("Beginning ML Gradient Calculation, Iteration %d.%d (%d)\n", 
427         // oo->matrix->currentState->majorIteration, oo->matrix->currentState->minorIteration,
428         // oo->matrix->currentState->computeCount); //:::DEBUG:::
429     // 1) Calculate current Expected Covariance
430     // 2) Calculate eCov, the Inverse Expected Covariance matrix 
431     // 3) Calculate C = I - eCov D, where D is the observed covariance matrix
432     // 4) Calculate b = M - [observed M]
433     // 5) For each location in locs:
434     //   gradient[loc] = tr(eCov^-1 %*% dEdt %*% C) - (b^T %*% eCov^-1 %*% dEdt + 2 dMdt^T))eCov^-1 b)
435
436     omxMLFitFunction *omo = ((omxMLFitFunction*)oo->argStruct);
437     
438     /* Locals for readability.  Compiler should cut through this. */
439     omxMatrix *scov         = omo->observedCov;
440     omxMatrix *smeans       = omo->observedMeans;
441     omxMatrix *cov          = omo->expectedCov;
442     omxMatrix *M            = omo->expectedMeans;
443     omxMatrix *eCov         = omo->localCov;        // TODO: Maybe need to avoid reusing these
444     omxMatrix *I            = omo->I;
445     omxMatrix *C            = omo->C;
446     omxMatrix *X            = omo->X;
447     omxMatrix *Y            = omo->Y;
448     omxMatrix *Mu           = omo->Mu;
449     omxMatrix *Ms           = omo->Ms;
450     omxMatrix *P            = omo->P;
451     double n                = omo->n;
452     omxMatrix** dSigmas     = omo->dSigma;
453     omxMatrix** dMus        = omo->dMu;
454     
455     size_t gradientSize = oo->freeVarGroup->vars.size();
456     
457     char u = 'U';
458     int info;
459     double minusoned = -1.0;
460     int onei = 1;
461     int status[gradientSize];
462     int nLocs = gradientSize;
463     
464     // Calculate current FitFunction values
465     // We can safely assume this has been done
466     // omxFitFunctionCompute(oo);
467     
468     // Calculate current eCov
469     
470     omxCopyMatrix(eCov, cov);                           // But expected cov is destroyed in inversion
471     
472     F77_CALL(dpotrf)(&u, &(eCov->cols), eCov->data, &(eCov->cols), &info);
473
474     if(OMX_DEBUG_ALGEBRA) { mxLog("Info on LU Decomp: %d", info);}
475     if(info > 0) {
476             char *errstr = (char*) calloc(250, sizeof(char));
477         sprintf(errstr, "Expected covariance matrix is non-positive-definite");
478         if(oo->matrix->currentState->computeCount <= 0) {
479             strncat(errstr, " at starting values", 20);
480         }
481         strncat(errstr, ".\n", 3);
482         omxRaiseError(oo->matrix->currentState, -1, errstr);                        // Raise error
483         free(errstr);
484         return;                                                                     // Leave output untouched
485     }
486     
487     F77_CALL(dpotri)(&u, &(eCov->rows), eCov->data, &(eCov->cols), &info);
488     if(info > 0) {
489             char *errstr = (char*) calloc(250, sizeof(char));
490         sprintf(errstr, "Expected covariance matrix is not invertible");
491         if(oo->matrix->currentState->computeCount <= 0) {
492             strncat(errstr, " at starting values", 20);
493         }
494         strncat(errstr, ".\n", 3);
495         omxRaiseError(oo->matrix->currentState, -1, errstr);                        // Raise error
496         free(errstr);
497         return;
498     }
499     // Calculate P = expected means - observed means
500     if(M != NULL) {
501         omxCopyMatrix(P, smeans);
502         F77_CALL(daxpy)(&(smeans->cols), &minusoned, M->data, &onei, P->data, &onei);
503     }
504         
505         // Reset C and Calculate C = I - eCov * oCov
506     omxCopyMatrix(C, I);
507     omxDSYMM(TRUE, -1.0, eCov, scov, 1.0, C);
508     
509     // For means, calculate Ms = eCov-1 %*% P
510     if(M != NULL)
511         omxDSYMM(FALSE, 1.0, eCov, P, 0.0, Ms);
512     
513     // Calculate parameter-level derivatives
514     // TODO: Parallelize Here.
515
516     if(OMX_DEBUG)  { mxLog("Calling component function."); }
517     omo->derivativeFun(oo, dSigmas, dMus, status);
518     
519     for(int currentLoc = 0; currentLoc < nLocs; currentLoc++) {
520         double meanInfluence, covInfluence;
521         if(status[currentLoc] < 0) continue;  // Failure in computation--skip.
522         //   gradient[loc] = tr(eCov^-1 %*% dEdt %*% C) - 
523         //    (b^T %*% eCov^-1 %*% dEdt + 2 dMdt^T))eCov^-1 b)
524         // omxDGEMM(FALSE, FALSE, 1.0, dSigmas[currentLoc], C, 0.0, Y);
525         omxDSYMM(TRUE, 1.0, dSigmas[currentLoc], C, 0.0, Y);
526         omxDSYMM(TRUE, 1.0, eCov, Y, 0.0, X);
527         gradient[currentLoc] = 0;
528         covInfluence = 0.0;
529         for(int i = 0; i < eCov->cols; i++) 
530             covInfluence += omxMatrixElement(X, i, i);
531         if(M != NULL) {
532             omxCopyMatrix(Mu, dMus[currentLoc]);
533             omxDSYMV(1.0, dSigmas[currentLoc], Ms, 2.0, Mu);
534             meanInfluence = F77_CALL(ddot)(&(eCov->cols), Mu->data, &onei, Ms->data, &onei);
535         } else {
536             meanInfluence = 0;
537         }
538         gradient[currentLoc] = (covInfluence * (n-1)) - (meanInfluence * n);
539         if(OMX_DEBUG) { 
540             mxLog("Calculation for Gradient value %d: Cov: %3.9f, Mean: %3.9f, total: %3.9f",
541             currentLoc, covInfluence, meanInfluence, gradient[currentLoc]); 
542         }
543     }
544 }