Rename omxFastRAMInverse and move to omxMatrix
[openmx:openmx.git] / src / omxRAMExpectation.c
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 "omxExpectation.h"
18 #include "omxFitFunction.h"
19 #include "omxBLAS.h"
20 #include "omxFIMLFitFunction.h"
21 #include "omxMLFitFunction.h"
22 #include "omxRAMExpectation.h"
23
24 void calculateRAMGradientComponents(omxExpectation* oo, omxMatrix**, omxMatrix**, int*);
25 void sliceCrossUpdate(omxMatrix* A, omxMatrix* B, int row, int col, omxMatrix* result);
26 void fastRAMGradientML(omxExpectation* oo, omxFitFunction* off, double* result);
27
28 // Speedup Helper
29 void ADB(omxMatrix** A, omxMatrix** B, int numArgs, omxMatrix** D, int *Dcounts, 
30         int *DrowCache, int *DcolCache, int matNum, omxFreeVar* varList, 
31         int* pNums, int nParam, omxMatrix*** result) {
32     // Computes Matrix %*% params %*% Matrix in O(K^2) time.  Based on von Oertzen & Brick, in prep.
33     // Also populates the matrices called D if it appears.
34     // Minimal error checking.
35     if(OMX_DEBUG_ALGEBRA) Rprintf("Beginning ADB.\n"); //:::DEBUG:::
36
37     omxFreeVar var;
38     int paramNo;
39
40     for(int param = 0; param < nParam; param++) {
41         if(pNums != NULL) {
42             paramNo = pNums[param];
43         } else {
44             paramNo = param;
45         }
46         var = varList[paramNo];
47
48         // Set D
49         if(D != NULL) {
50             int nonzero = 0;
51             omxMatrix *thisD = D[param];
52             memset(thisD->data, 0, sizeof(double) * thisD->cols * thisD->rows);
53             // Honestly, this should be calculated only once.
54             for(int varLoc = 0; varLoc < var.numLocations; varLoc++) {
55                 if(~var.matrices[varLoc] == matNum) {
56                     omxSetMatrixElement(thisD, var.row[varLoc], var.col[varLoc], 1.0);
57                     if (DrowCache != NULL) DrowCache[param] = var.row[varLoc];
58                     if (DcolCache != NULL) DcolCache[param] = var.col[varLoc];
59                     nonzero++;
60                 }
61             }
62             Dcounts[param] = nonzero;
63         }
64         for(int eqn = 0; eqn < numArgs; eqn++) {
65             omxMatrix* thisResult = result[eqn][param];
66             memset(thisResult->data, 0, sizeof(double) * thisResult->cols * thisResult->rows);
67             for(int varLoc = 0; varLoc < var.numLocations; varLoc++) {
68                 if(~var.matrices[varLoc] == matNum) {
69                     sliceCrossUpdate(A[eqn], B[eqn], var.row[varLoc], var.col[varLoc], thisResult);
70                 }
71             }
72         }
73     }
74 }
75
76 void sliceCrossUpdate(omxMatrix* A, omxMatrix* B, int row, int col, omxMatrix* result) {
77     // Performs outer multiply of column col of A by row row of B.
78     // Adds product to beta * result.
79     int nrow = A->rows;
80     int ncol = B->cols;
81     int resultrows = result->rows;
82     int brows = B->rows;
83
84     if (A->colMajor && B->colMajor && result->colMajor) {
85         int aoffset = row * nrow;
86         double* Adata = A->data;
87         double* Bdata = B->data;
88         double* resdata = result->data;
89
90         for(int k = 0; k < ncol; k++) {
91             int offset = k * resultrows;
92             double factor = Bdata[k * brows + col];
93             for(int j = 0; j < nrow; j++) {
94                 resdata[offset + j] += Adata[aoffset + j] * factor;
95             }
96         }
97     } else {
98         for(int k = 0; k < ncol; k++) {
99             for(int j = 0; j < nrow; j++) {
100                 omxAccumulateMatrixElement(result, j, k, 
101                     omxMatrixElement(A, j, row) * omxMatrixElement(B, col, k));
102             }
103         }
104     }
105 }
106
107 double trAB(omxMatrix* A, omxMatrix* B) {
108     double output = 0.0;
109     // Computes tr(AB)
110     // Note: does not check for conformability
111     for(int j = 0; j < A->rows; j++) 
112         for(int k = 0; k < A->cols; k++)
113             output += omxMatrixElement(A, j, k) * omxMatrixElement(B, k, j);
114     
115     return(output);
116 }
117
118 void omxCallRAMExpectation(omxExpectation* oo) {
119     if(OMX_DEBUG) { Rprintf("RAM Expectation calculating.\n"); }
120         omxRAMExpectation* oro = (omxRAMExpectation*)(oo->argStruct);
121         
122         omxRecompute(oro->A);
123         omxRecompute(oro->S);
124         omxRecompute(oro->F);
125         if(oro->M != NULL)
126             omxRecompute(oro->M);
127             
128         omxCalculateRAMCovarianceAndMeans(oro->A, oro->S, oro->F, oro->M, oro->cov, 
129                 oro->means, oro->numIters, oro->I, oro->Z, oro->Y, oro->X, oro->Ax);
130 }
131
132 void omxDestroyRAMExpectation(omxExpectation* oo) {
133
134         if(OMX_DEBUG) { Rprintf("Destroying RAM Expectation.\n"); }
135         
136         omxRAMExpectation* argStruct = (omxRAMExpectation*)(oo->argStruct);
137
138         /* We allocated 'em, so we destroy 'em. */
139         if(argStruct->cov != NULL)
140                 omxFreeMatrixData(argStruct->cov);
141
142         if(argStruct->means != NULL) {
143                 omxFreeMatrixData(argStruct->dM);
144                 omxFreeMatrixData(argStruct->ZM);
145                 omxFreeMatrixData(argStruct->bCB);
146                 omxFreeMatrixData(argStruct->b);
147                 omxFreeMatrixData(argStruct->tempVec);
148                 omxFreeMatrixData(argStruct->bigSum);
149                 omxFreeMatrixData(argStruct->lilSum);
150                 omxFreeMatrixData(argStruct->beCov);
151                 omxFreeMatrixData(argStruct->means);
152         }
153
154         int nParam = argStruct->nParam;
155         if(nParam >= 0) {
156                 for(int j = 0; j < nParam; j++) {
157                         if(argStruct->dAdts != NULL)
158                                 omxFreeMatrixData(argStruct->dAdts[j]);
159                         if(argStruct->dSdts != NULL)
160                                 omxFreeMatrixData(argStruct->dSdts[j]);
161                         if(argStruct->dMdts != NULL) 
162                                 omxFreeMatrixData(argStruct->dMdts[j]);
163                         if(argStruct->eqnOuts != NULL)
164                                 omxFreeMatrixData(argStruct->eqnOuts[0][j]);
165                 }
166                 omxFreeMatrixData(argStruct->paramVec);
167         }
168
169         if (argStruct->dA != NULL)
170                 omxFreeMatrixData(argStruct->dA);
171
172         if (argStruct->dS != NULL)
173                 omxFreeMatrixData(argStruct->dS);
174
175         omxFreeMatrixData(argStruct->I);
176         omxFreeMatrixData(argStruct->lilI);
177         omxFreeMatrixData(argStruct->X);
178         omxFreeMatrixData(argStruct->Y);
179         omxFreeMatrixData(argStruct->Z);
180         omxFreeMatrixData(argStruct->Ax);
181
182         omxFreeMatrixData(argStruct->W);
183         omxFreeMatrixData(argStruct->U);
184         omxFreeMatrixData(argStruct->EF);
185         omxFreeMatrixData(argStruct->V);
186         omxFreeMatrixData(argStruct->ZSBC);
187         omxFreeMatrixData(argStruct->C);
188         omxFreeMatrixData(argStruct->eCov);
189
190 }
191
192 void omxPopulateRAMAttributes(omxExpectation *oo, SEXP algebra) {
193     if(OMX_DEBUG) { Rprintf("Populating RAM Attributes.\n"); }
194
195         omxRAMExpectation* oro = (omxRAMExpectation*) (oo->argStruct);
196         omxMatrix* A = oro->A;
197         omxMatrix* S = oro->S;
198         omxMatrix* X = oro->X;
199         omxMatrix* Ax= oro->Ax;
200         omxMatrix* Z = oro->Z;
201         omxMatrix* I = oro->I;
202     int numIters = oro->numIters;
203     double oned = 1.0, zerod = 0.0;
204     
205     omxRecompute(A);
206     omxRecompute(S);
207         
208         omxShallowInverse(numIters, A, Z, Ax, I ); // Z = (I-A)^-1
209         
210         if(OMX_DEBUG_ALGEBRA) { Rprintf("....DGEMM: %c %c \n %d %d %d \n %f \n %x %d %x %d \n %f %x %d.\n", *(Z->majority), *(S->majority), (Z->rows), (S->cols), (S->rows), oned, Z->data, (Z->leading), S->data, (S->leading), zerod, Ax->data, (Ax->leading));}
211         // F77_CALL(omxunsafedgemm)(Z->majority, S->majority, &(Z->rows), &(S->cols), &(S->rows), &oned, Z->data, &(Z->leading), S->data, &(S->leading), &zerod, Ax->data, &(Ax->leading));     // X = ZS
212         omxDGEMM(FALSE, FALSE, oned, Z, S, zerod, Ax);
213
214         if(OMX_DEBUG_ALGEBRA) { Rprintf("....DGEMM: %c %c %d %d %d %f %x %d %x %d %f %x %d.\n", *(Ax->majority), *(Z->minority), (Ax->rows), (Z->rows), (Z->cols), oned, X->data, (X->leading), Z->data, (Z->lagging), zerod, Ax->data, (Ax->leading));}
215         // F77_CALL(omxunsafedgemm)(Ax->majority, Z->minority, &(Ax->rows), &(Z->rows), &(Z->cols), &oned, Ax->data, &(Ax->leading), Z->data, &(Z->leading), &zerod, Ax->data, &(Ax->leading)); 
216         omxDGEMM(FALSE, TRUE, oned, Ax, Z, zerod, Ax);
217         // Ax = ZSZ' = Covariance matrix including latent variables
218         
219         SEXP expCovExt;
220         PROTECT(expCovExt = allocMatrix(REALSXP, Ax->rows, Ax->cols));
221         for(int row = 0; row < Ax->rows; row++)
222                 for(int col = 0; col < Ax->cols; col++)
223                         REAL(expCovExt)[col * Ax->rows + row] =
224                                 omxMatrixElement(Ax, row, col);
225         setAttrib(algebra, install("UnfilteredExpCov"), expCovExt);
226         UNPROTECT(1);
227 }
228
229 /*
230  * omxCalculateRAMCovarianceAndMeans
231  *                      Just like it says on the tin.  Calculates the mean and covariance matrices
232  * for a RAM model.  M is the number of total variables, latent and manifest. N is
233  * the number of manifest variables.
234  *
235  * params:
236  * omxMatrix *A, *S, *F         : matrices as specified in the RAM model.  MxM, MxM, and NxM
237  * omxMatrix *M                         : vector containing model implied means. 1xM
238  * omxMatrix *Cov                       : On output: model-implied manifest covariance.  NxN.
239  * omxMatrix *Means                     : On output: model-implied manifest means.  1xN.
240  * int numIterations            : Precomputed number of iterations of taylor series expansion.
241  * omxMatrix *I                         : Identity matrix.  If left NULL, will be populated.  MxM.
242  * omxMatrix *Z                         : On output: Computed (I-A)^-1. MxM.
243  * omxMatrix *Y, *X, *Ax        : Space for computation. NxM, NxM, MxM.  On exit, populated.
244  */
245
246 void omxCalculateRAMCovarianceAndMeans(omxMatrix* A, omxMatrix* S, omxMatrix* F, 
247         omxMatrix* M, omxMatrix* Cov, omxMatrix* Means, int numIters, omxMatrix* I, 
248         omxMatrix* Z, omxMatrix* Y, omxMatrix* X, omxMatrix* Ax) {
249         
250         if(OMX_DEBUG) { Rprintf("Running RAM computation with numIters is %d\n.", numIters); }
251                 
252         double oned = 1.0, zerod=0.0;
253         
254         if(Ax == NULL || I == NULL || Z == NULL || Y == NULL || X == NULL) {
255                 error("Internal Error: RAM Metadata improperly populated.  Please report this to the OpenMx development team.");
256         }
257                 
258         if(Cov == NULL && Means == NULL) {
259                 return; // We're not populating anything, so why bother running the calculation?
260         }
261         
262         // if(   (Cov->rows != Cov->cols)  || (A->rows  != A->cols)  // Conformance check
263         //      || (X->rows  != Cov->cols)  || (X->cols  != A->rows)
264         //      || (Y->rows  != Cov->cols)  || (Y->cols  != A->rows)
265         //      || (Ax->rows != Cov->cols)  || (Ax->cols != A->rows)
266         //      || (I->rows  != Cov->cols)  || (I->cols  != Cov->rows)
267         //      || (Y->rows  != Cov->cols)  || (Y->cols  != A->rows)
268         //      || (M->cols  != Cov->cols)  || (M->rows  != 1)
269         //      || (Means->rows != 1)       || (Means->cols != Cov->cols) ) {
270         //              error("INTERNAL ERROR: Incorrectly sized matrices being passed to omxRAMExpectation Calculation.\n Please report this to the OpenMx development team.");
271         // }
272         
273         omxShallowInverse(numIters, A, Z, Ax, I );
274
275         if(OMX_DEBUG_ALGEBRA) Rprintf("Status is %d\n", A->currentState->statusCode);
276         
277     if(A->currentState->statusCode < 0) return;
278         
279         /* Cov = FZSZ'F' */
280         if(OMX_DEBUG_ALGEBRA) { Rprintf("....DGEMM: %c %c %d %d %d %f %x %d %x %d %f %x %d.\n", *(F->majority), *(Z->majority), (F->rows), (Z->cols), (Z->rows), oned, F->data, (F->leading), Z->data, (Z->leading), zerod, Y->data, (Y->leading));}
281         // F77_CALL(omxunsafedgemm)(F->majority, Z->majority, &(F->rows), &(Z->cols), &(Z->rows), &oned, F->data, &(F->leading), Z->data, &(Z->leading), &zerod, Y->data, &(Y->leading));       // Y = FZ
282         omxDGEMM(FALSE, FALSE, 1.0, F, Z, 0.0, Y);
283
284         if(OMX_DEBUG_ALGEBRA) { Rprintf("....DGEMM: %c %c %d %d %d %f %x %d %x %d %f %x %d.\n", *(Y->majority), *(S->majority), (Y->rows), (S->cols), (S->rows), oned, Y->data, (Y->leading), S->data, (S->leading), zerod, X->data, (X->leading));}
285         // F77_CALL(omxunsafedgemm)(Y->majority, S->majority, &(Y->rows), &(S->cols), &(S->rows), &oned, Y->data, &(Y->leading), S->data, &(S->leading), &zerod, X->data, &(X->leading));       // X = FZS
286         omxDGEMM(FALSE, FALSE, 1.0, Y, S, 0.0, X);
287
288         if(OMX_DEBUG_ALGEBRA) { Rprintf("....DGEMM: %c %c %d %d %d %f %x %d %x %d %f %x %d.\n", *(X->majority), *(Y->minority), (X->rows), (Y->rows), (Y->cols), oned, X->data, (X->leading), Y->data, (Y->lagging), zerod, Cov->data, (Cov->leading));}
289         // F77_CALL(omxunsafedgemm)(X->majority, Y->minority, &(X->rows), &(Y->rows), &(Y->cols), &oned, X->data, &(X->leading), Y->data, &(Y->leading), &zerod, Cov->data, &(Cov->leading));
290         omxDGEMM(FALSE, TRUE, 1.0, X, Y, 0.0, Cov);
291          // Cov = FZSZ'F' (Because (FZ)' = Z'F')
292         
293         if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Cov, "....RAM: Model-implied Covariance Matrix:");}
294         
295         if(M != NULL && Means != NULL) {
296                 // F77_CALL(omxunsafedgemv)(Y->majority, &(Y->rows), &(Y->cols), &oned, Y->data, &(Y->leading), M->data, &onei, &zerod, Means->data, &onei);
297                 omxDGEMV(FALSE, 1.0, Y, M, 0.0, Means);
298                 if(OMX_DEBUG_ALGEBRA) {omxPrintMatrix(Means, "....RAM: Model-implied Means Vector:");}
299         }
300 }
301
302 void omxInitRAMExpectation(omxExpectation* oo, SEXP rObj) {
303         
304         omxState* currentState = oo->currentState;      
305
306     if(OMX_DEBUG) { Rprintf("Initializing RAM expectation.\n"); }
307         
308         int l, k;
309
310         SEXP slotValue;
311         
312         omxRAMExpectation *RAMexp = (omxRAMExpectation*) R_alloc(1, sizeof(omxRAMExpectation));
313         oo->expType = "omxRAMExpectation";
314         
315         /* Set Expectation Calls and Structures */
316         oo->computeFun = omxCallRAMExpectation;
317         oo->destructFun = omxDestroyRAMExpectation;
318         oo->componentFun = omxGetRAMExpectationComponent;
319         oo->populateAttrFun = omxPopulateRAMAttributes;
320         oo->argStruct = (void*) RAMexp;
321         
322         /* Set up expectation structures */
323         if(OMX_DEBUG) { Rprintf("Initializing RAM expectation.\n"); }
324
325         if(OMX_DEBUG) { Rprintf("Processing M.\n"); }
326         RAMexp->M = omxNewMatrixFromIndexSlot(rObj, currentState, "M");
327
328         if(OMX_DEBUG) { Rprintf("Processing A.\n"); }
329         RAMexp->A = omxNewMatrixFromIndexSlot(rObj, currentState, "A");
330
331         if(OMX_DEBUG) { Rprintf("Processing S.\n"); }
332         RAMexp->S = omxNewMatrixFromIndexSlot(rObj, currentState, "S");
333
334         if(OMX_DEBUG) { Rprintf("Processing F.\n"); }
335         RAMexp->F = omxNewMatrixFromIndexSlot(rObj, currentState, "F");
336
337         if(OMX_DEBUG) { Rprintf("Processing usePPML.\n"); }
338         PROTECT(slotValue = GET_SLOT(rObj, install("usePPML")));
339         UNPROTECT(1);
340
341         /* Identity Matrix, Size Of A */
342         if(OMX_DEBUG) { Rprintf("Generating I.\n"); }
343         RAMexp->I = omxNewIdentityMatrix(RAMexp->A->rows, currentState);
344         omxRecompute(RAMexp->I);
345         RAMexp->lilI = omxNewIdentityMatrix(RAMexp->F->rows, currentState);
346         omxRecompute(RAMexp->lilI);
347         
348
349         if(OMX_DEBUG) { Rprintf("Processing expansion iteration depth.\n"); }
350         PROTECT(slotValue = GET_SLOT(rObj, install("depth")));
351         RAMexp->numIters = INTEGER(slotValue)[0];
352         if(OMX_DEBUG) { Rprintf("Using %d iterations.", RAMexp->numIters); }
353         UNPROTECT(1);
354
355         l = RAMexp->F->rows;
356         k = RAMexp->A->cols;
357
358         if(OMX_DEBUG) { Rprintf("Generating internals for computation.\n"); }
359
360         RAMexp->Z =     omxInitMatrix(NULL, k, k, TRUE, currentState);
361         RAMexp->Ax =    omxInitMatrix(NULL, k, k, TRUE, currentState);
362         RAMexp->W =     omxInitMatrix(NULL, k, k, TRUE, currentState);
363         RAMexp->U =     omxInitMatrix(NULL, l, k, TRUE, currentState);
364         RAMexp->Y =     omxInitMatrix(NULL, l, k, TRUE, currentState);
365         RAMexp->X =     omxInitMatrix(NULL, l, k, TRUE, currentState);
366         RAMexp->EF=     omxInitMatrix(NULL, k, l, TRUE, currentState);
367         RAMexp->V =     omxInitMatrix(NULL, l, k, TRUE, currentState);
368         RAMexp->ZSBC =  omxInitMatrix(NULL, k, l, TRUE, currentState);
369         RAMexp->C =     omxInitMatrix(NULL, l, l, TRUE, currentState);
370         
371         if(omxIsMatrix(RAMexp->A)) {
372                 RAMexp->dA = omxInitMatrix(NULL, k, k, TRUE, currentState);
373         } else {
374                 RAMexp->dA = NULL;
375         }
376         if(omxIsMatrix(RAMexp->S)) {
377                 RAMexp->dS = omxInitMatrix(NULL, k, k, TRUE, currentState);
378         } else {
379                 RAMexp->dS = NULL;
380         }
381         if(RAMexp->M != NULL) {
382                 RAMexp->dM =    omxInitMatrix(NULL, 1, k, TRUE, currentState);
383                 RAMexp->ZM =    omxInitMatrix(NULL, k, 1, TRUE, currentState);
384                 RAMexp->bCB =   omxInitMatrix(NULL, k, 1, TRUE, currentState);
385                 RAMexp->b =     omxInitMatrix(NULL, l, 1, TRUE, currentState);
386                 RAMexp->tempVec =       omxInitMatrix(NULL, k, 1, TRUE, currentState);
387                 RAMexp->bigSum =        omxInitMatrix(NULL, k, 1, TRUE, currentState);
388                 RAMexp->lilSum =        omxInitMatrix(NULL, l, 1, TRUE, currentState);
389                 RAMexp->beCov =         omxInitMatrix(NULL, l, 1, TRUE, currentState);
390                 RAMexp->Mns =   NULL;
391     }
392
393         RAMexp->cov =           omxInitMatrix(NULL, l, l, TRUE, currentState);
394
395         if(RAMexp->M != NULL) {
396                 RAMexp->means =         omxInitMatrix(NULL, 1, l, TRUE, currentState);
397         } else {
398             RAMexp->means  =    NULL;
399     }
400
401     // Derivative space:
402         RAMexp->eqnOuts = NULL;
403         RAMexp->dAdts = NULL;
404         RAMexp->dSdts = NULL;
405         RAMexp->dMdts = NULL;
406         RAMexp->paramVec = NULL;
407         RAMexp->D =     NULL;
408         RAMexp->pNums = NULL;
409         RAMexp->nParam = -1;
410         RAMexp->eCov=       omxInitMatrix(NULL, l, l, TRUE, currentState);
411
412 }
413
414 void omxExpectationSetFitFunction(omxExpectation *ox, omxFitFunction *off) {
415 /* Check for fit and make any changes based on the type of the underlying object */
416
417         omxRAMExpectation* RAMexp = (omxRAMExpectation*)(ox->argStruct);
418         // For fast gradient calculations
419         // If this block doesn't execute, we don't use gradients.
420         if(!strncmp("omxMLFitFunction", off->fitType, 14)) {
421                 // Mode switch here.  Faster one is:
422                 // omxSetMLFitFunctionGradient(off, fastRAMGradientML);
423                 // Otherwise, call 
424                 // omxSetMLFitFunctionGradientComponents(oo, calculateRAMGradientComponents);
425                 // Note that once FIML kicks in, components should still work.
426                 // Probably, we'll use the same thing for WLS.
427                 RAMexp->D = ((omxMLFitFunction*)(off->argStruct))->observedCov;
428                 RAMexp->Mns = ((omxMLFitFunction*)(off->argStruct))->observedMeans;
429                 RAMexp->n = ((omxMLFitFunction*)(off->argStruct))->n;
430     }
431 }
432
433 static inline void omxVectorMultiplyAndSet(double *restrict dest, 
434                                            double *restrict src, 
435                                            int len, double value) {
436
437     for(int offset = 0; offset < len; offset++)
438         dest[offset] = src[offset] * value;
439
440 }
441
442
443
444 /*
445  * fastRAMGradientML
446  *                      Calculates the derivatives of the likelihood for a RAM model.  
447  * Locations of free parameters are extracted from the omxExpecation's state object.
448  *
449  * params:
450  * omxMatrix *A, *S, *F         : matrices as specified in the RAM model.  MxM, MxM, and NxM
451  * omxMatrix *M                         : vector containing model implied means. 1xM
452  * omxMatrix *Cov                       : On output: model-implied manifest covariance.  NxN.
453  * omxMatrix *Means                     : On output: model-implied manifest means.  1xN.
454  * int numIterations            : Precomputed number of iterations of taylor series expansion.
455  * omxMatrix *I                         : Identity matrix.  If left NULL, will be populated.  MxM.
456  * omxMatrix *Z                         : On output: Computed (I-A)^-1. MxM.
457  * omxMatrix *Y, *X, *Ax        : Space for computation. NxM, NxM, MxM.  On exit, populated.
458  */
459
460 void fastRAMGradientML(omxExpectation* oo, omxFitFunction* off, double* result) {
461     
462     // This calculation is based on von Oertzen and Brick, in prep.
463     //
464     // Steps:
465     // 1) (Re) Calculate current A, S, F, M, and Z (where Z = (I-A)^-1)
466     // 2) cov = current Expected Covariance (fxf)
467     // 3) eCov = cov^(-1) (fxf) *
468     // 4) ZS = Z S  (axa) *
469     // 5) B = F Z   (fxa) *
470     // 6) C = I - eCov D, where D is the observed covariance matrix (fxf) *
471     // 7) BC = B^T C (axf) *
472     // 8) ZSB = ZS (B^T) *
473     // 8) ZSBC = ZS BC (axf) *
474     // 9) eCovB = cov^(-1) B (fxa) *
475     // For Means:
476         // 10) Means = BM (fx1) *
477         // 11) b = Means - d, where d is the observed means matrix (fx1)
478         // 12) beCov = b cov^(-1) (1xf) *
479         // 13) beCovB = b cov^(-1) B (1xa) *
480
481     
482     // 14) For each location in locations: (Note: parallelize, gradients here!)
483     //      1A) Calculate dA/dt and dS/dt by substituting 1s into empty matrices
484     //      1B) eqnList = 2 * ADB(eCovB, dAdt, ZSBC)
485     //      1C) eqnList2 = ADB(eCovB, dSdt, BC)  // Preferably, build
486     //      2) sum = eqnList1 + eqnList2
487     //      If Means:
488     //          3) sumVec = beCovB dAdt ZS
489     //          4) sumVec += beCovB ZS dAdt
490     //          5) sumVec += beCovB dSdt
491     //          6) sumVec *= B^T
492     //          7) sumVec += 2 * B dAdt Means
493     //          8) sumVec += 2 * B dMdt
494     //          9) sum += sumVec beCov
495
496     //  Right.  All the * elements are saved for hessian computation.
497  
498     if(OMX_DEBUG) {Rprintf("Calculating fast RAM/ML gradient.\n"); }
499     
500     omxRAMExpectation* oro = (omxRAMExpectation*)(oo->argStruct);
501                                     // Size reference: A is axa, F is fxa
502     omxMatrix* A = oro->A;          // axa
503     omxMatrix* S = oro->S;          // axa
504     omxMatrix* F = oro->F;          // fxa
505     omxMatrix* M = oro->M;          // 1xa
506     omxMatrix* Z = oro->Z;          // axa
507
508     omxMatrix* cov = oro->cov;      // fxf
509     omxMatrix* means = oro->means;  // fx1
510     omxMatrix* eCov = oro->eCov;    // fxf
511
512         omxMatrix* ZS = oro->Ax;        // axa
513         
514     omxMatrix* B = oro->X;          // fxa
515     omxMatrix* C = oro->C;          // fxf
516     omxMatrix* BC = oro->EF;        // fxa
517     omxMatrix* ZSBC = oro->ZSBC;    // fxa
518     omxMatrix* eCovB = oro->U;      // fxa
519
520     omxMatrix* ZM = oro->ZM;        // 1xa
521     omxMatrix* beCov = oro->beCov;  // 1xf
522     omxMatrix* bCB = oro->bCB;      // 1xa
523     omxMatrix* b = oro->b;          // 1xa
524     
525     omxMatrix** dAdts = oro->dAdts;
526     omxMatrix** dSdts = oro->dSdts;
527     omxMatrix** dMdts = oro->dMdts;
528     
529     int* dAdtsCount = oro->dAdtsCount;
530     int* dSdtsCount = oro->dSdtsCount;
531     int* dMdtsCount = oro->dMdtsCount;
532
533     int* dAdtsRowCache = oro->dAdtsRowCache;
534     int* dAdtsColCache = oro->dAdtsColCache;
535     int* dSdtsRowCache = oro->dSdtsRowCache;
536     int* dSdtsColCache = oro->dSdtsColCache;
537     int* dMdtsRowCache = oro->dMdtsRowCache;
538     int* dMdtsColCache = oro->dMdtsColCache;
539
540     omxMatrix*** eqnOuts = oro->eqnOuts;
541     
542     omxMatrix* tempVec = oro->tempVec;
543     omxMatrix* bigSum  = oro->bigSum;
544     omxMatrix* lilSum  = oro->lilSum;
545     omxMatrix* D = oro->D;
546     omxMatrix* Mns = oro->Mns;
547     
548     omxMatrix* lilI = oro->lilI;
549     omxMatrix* paramVec = oro->paramVec;
550     
551     double n = oro->n;
552     
553     int* pNums = oro->pNums;
554     int nParam = oro->nParam;
555     
556     int Amat = A->matrixNumber;
557     int Smat = S->matrixNumber;
558     int Mmat = 0;
559     double oned = 1.0, minusoned = -1.0;
560     int onei = 1;
561     int info;
562     char u = 'U';
563     if(M != NULL) Mmat = M->matrixNumber;
564     
565     omxFreeVar* varList = oo->currentState->freeVarList;
566
567     omxMatrix *eqnList1[1], *eqnList2[1];
568
569     if(nParam < 0) {
570         nParam = 0;
571         int nTotalParams = oo->currentState->numFreeParams;
572         if(OMX_DEBUG) { Rprintf("Planning Memory for Fast Gradient Calculation: Using %d params.\n", nParam); }
573         unsigned short int calc[nTotalParams]; 
574         // Work out the set of parameters for which we can calculate gradients
575         // TODO: Potential speedup by splitting this to calculate dA, dS, and dM separately
576         for(int parm = 0; parm < nTotalParams; parm++) {
577             omxFreeVar ofv = varList[parm];
578             calc[parm] = 0;
579             for(int loc = 0; loc < ofv.numLocations; loc++) {
580                 int varMat = ~(ofv.matrices[loc]);
581                 if(varMat == Amat || varMat == Smat || (M != NULL && varMat == Mmat)) {
582                     calc[parm] = 1;
583                 } else {
584                     calc[parm] = 0;
585                     break;
586                 }
587             }
588             if(calc[parm]) {
589                 nParam++;
590             }
591         }
592         oro->nParam = nParam;
593         pNums = (int*)R_alloc(nParam, sizeof(int));
594
595         int nextFree = 0;
596         // Populate pNums with numbers of the parameters to calculate
597         for(int parm = 0; parm < nTotalParams && nextFree < nParam; parm++) {
598             if(calc[parm]) {
599                 pNums[nextFree] = parm;
600                 nextFree++;
601
602             }
603         }
604         oro->pNums = pNums;
605         
606         oro->dAdts = (omxMatrix**) R_alloc(nParam, sizeof(omxMatrix*));
607         oro->dSdts = (omxMatrix**) R_alloc(nParam, sizeof(omxMatrix*));
608         oro->dMdts = (omxMatrix**) R_alloc(nParam, sizeof(omxMatrix*));
609         oro->dAdtsCount = (int*) R_alloc(nParam, sizeof(int));
610         oro->dSdtsCount = (int*) R_alloc(nParam, sizeof(int));
611         oro->dMdtsCount = (int*) R_alloc(nParam, sizeof(int));
612         
613         oro->dAdtsRowCache = (int*) R_alloc(nParam, sizeof(int));
614         oro->dAdtsColCache = (int*) R_alloc(nParam, sizeof(int));
615         oro->dSdtsRowCache = (int*) R_alloc(nParam, sizeof(int));
616         oro->dSdtsColCache = (int*) R_alloc(nParam, sizeof(int));
617         oro->dMdtsRowCache = (int*) R_alloc(nParam, sizeof(int));
618         oro->dMdtsColCache = (int*) R_alloc(nParam, sizeof(int));
619
620         oro->eqnOuts = (omxMatrix***) R_alloc(1, sizeof(omxMatrix**));
621         oro->eqnOuts[0] = (omxMatrix**) R_alloc(nParam, sizeof(omxMatrix*));
622         
623         int a = A->rows;
624         omxState* currentState = oo->currentState;
625         for(int i = 0; i < nParam; i++) {
626             oro->dAdts[i] = omxInitMatrix(NULL, a, a, TRUE, currentState);
627             oro->dSdts[i] = omxInitMatrix(NULL, a, a, TRUE, currentState);
628             oro->dMdts[i] = omxInitMatrix(NULL, 1, a, TRUE, currentState);
629             
630             oro->eqnOuts[0][i] = omxInitMatrix(NULL, a, a, TRUE, currentState);
631         }
632         oro->paramVec = omxInitMatrix(NULL, nParam, 1, TRUE, currentState);
633         paramVec = oro->paramVec;
634         dAdts = oro->dAdts;
635         dSdts = oro->dSdts;
636         dMdts = oro->dMdts;
637         dAdtsCount = oro->dAdtsCount;
638         dSdtsCount = oro->dSdtsCount;
639         dMdtsCount = oro->dMdtsCount;
640         
641         dAdtsRowCache = oro->dAdtsRowCache;
642         dAdtsColCache = oro->dAdtsColCache;
643         dSdtsRowCache = oro->dSdtsRowCache;
644         dSdtsColCache = oro->dSdtsColCache;
645         dMdtsRowCache = oro->dMdtsRowCache;
646         dMdtsColCache = oro->dMdtsColCache;
647
648         eqnOuts = oro->eqnOuts;
649     }
650
651     double covInfluence[nParam];
652     double meanInfluence[nParam];
653     
654     // 1) (Re) Calculate current A, S, F, M, and Z (where Z = (I-A)^-1)
655     // 2) cov = current Expected Covariance (fxf)
656     // Theoretically, we should calculate current fit function values 
657     // But we can safely assume this has already been done
658     // That assumption may no longer be valid in FIML.
659
660     // 3) eCov = cov^(-1) (fxf)
661     omxCopyMatrix(eCov, cov);                           // But expected cov is destroyed in inversion
662     
663     F77_CALL(dpotrf)(&u, &(eCov->cols), eCov->data, &(eCov->cols), &info);
664
665     if(OMX_DEBUG_ALGEBRA) { Rprintf("Info on LU Decomp: %d\n", info);}
666     if(info > 0) {
667         char *errstr = calloc(250, sizeof(char));
668         sprintf(errstr, "Expected covariance matrix is non-positive-definite");
669         if(oo->currentState->computeCount <= 0) {
670             strncat(errstr, " at starting values", 20);
671         }
672         strncat(errstr, ".\n", 3);
673         omxRaiseError(oo->currentState, -1, errstr);                        // Raise error
674         free(errstr);
675         return;                                                                     // Leave output untouched
676     }
677     
678     F77_CALL(dpotri)(&u, &(eCov->rows), eCov->data, &(eCov->cols), &info);
679     if(info > 0) {
680         char *errstr = calloc(250, sizeof(char));
681         sprintf(errstr, "Expected covariance matrix is not invertible");
682         if(oo->currentState->computeCount <= 0) {
683             strncat(errstr, " at starting values", 20);
684         }
685         strncat(errstr, ".\n", 3);
686         omxRaiseError(oo->currentState, -1, errstr);                        // Raise error
687         free(errstr);
688         return;
689     }
690
691     // 4) ZS = Z S  (axa) *
692     omxDSYMM(FALSE, 1.0, S, Z, 0.0, ZS);
693     omxDGEMM(FALSE, FALSE, 1.0, Z, S, 0.0, ZS);
694     
695     // 5) B = F Z   (fxa) *
696     omxDGEMM(FALSE, FALSE, 1.0, F, Z, 0.0, B);
697     
698     // 6) C = I - eCov D, where D is the observed covariance matrix (fxf)
699     omxCopyMatrix(C, lilI);
700     omxDSYMM(TRUE, -1.0, eCov, D, 1.0, C);
701
702     
703     // 7) BC = B^T C (axf) *
704     omxDGEMM(TRUE, FALSE, 1.0, B, C, 0.0, BC);
705     
706     // 8) ZSBC = ZS BC (axf) *
707     omxDGEMM(FALSE, FALSE, 1.0, ZS, BC, 0.0, ZSBC);
708     
709     // 9) eCovB = cov^(-1) B (fxa) *
710     omxDSYMM(TRUE, 1.0, eCov, B, 0.0, eCovB);
711     
712     if(M != NULL) {
713         // 10) Means = BM (fx1) *
714         // This is calculated during expectation computation.
715         // 11) b = Means - d, where d is the observed means matrix (fx1)
716
717         omxCopyMatrix(b, Mns);
718         F77_CALL(daxpy)(&(means->cols), &minusoned, means->data, &onei, b->data, &onei);
719     
720         // 11) beCov = b cov^(-1) (1xf) *
721         omxDSYMV(1.0, eCov, b, 0.0, beCov);
722         
723         // 12) beCovB = b cov^(-1) B (1xa) *
724         omxDGEMV(TRUE, 1.0, eCovB, b, 0.0, bCB);
725         
726         // 13) ZM = Z %*% M (1xa)
727         omxDGEMV(FALSE, 1.0, Z, M, 0.0, ZM);
728     }
729     
730     // 14) For each location in locations:
731     //      1A) Calculate dA/dt, dS/dt, and dM/dt by substituting 1s into empty matrices
732     //      1B) 1 = 2 * ADB(eCovB, dAdt, ZSBC)
733     
734     eqnList1[0] = eCovB;
735     eqnList2[0] = ZSBC;
736     ADB(eqnList1, eqnList2, 1, dAdts, dAdtsCount, dAdtsRowCache, dAdtsColCache, 
737         Amat, varList, pNums, nParam, eqnOuts);
738     omxMatrixTrace(eqnOuts[0], nParam, paramVec);
739     
740     for(int i = 0; i < nParam; i++)
741         covInfluence[i] = 2 * omxVectorElement(paramVec, i);
742         
743     
744     //      1C) eqnList2 = ADB(eCovB, dSdt, BC)
745     eqnList1[0] = eCovB;
746     eqnList2[0] = BC;
747     ADB(eqnList1, eqnList2, 1, dSdts, dSdtsCount, dSdtsRowCache, dSdtsColCache, 
748         Smat, varList, pNums, nParam, eqnOuts);
749     omxMatrixTrace(eqnOuts[0], nParam, paramVec);
750
751     //      2) sum = eqnList1 + eqnList2
752     F77_CALL(daxpy)(&nParam, &oned, paramVec->data, &onei, covInfluence, &onei);
753
754     //      If Means:
755     if(M != NULL) {
756
757         //      Just populate dMdts
758         ADB(NULL, NULL, 0, dMdts, dMdtsCount, dMdtsRowCache, dMdtsColCache,
759             Mmat, varList, pNums, nParam, NULL);
760
761         //  This needs to be done one-per-param.
762         //  Parallelize here.
763         for(int pNum = 0; pNum < nParam; pNum++) {
764
765             switch(dAdtsCount[pNum]) {
766                 case 0:
767                     switch(dSdtsCount[pNum]) {
768                         case 0:
769                             memset(bigSum->data, 0, sizeof(double) * bigSum->rows);
770                             break;
771                         case 1:
772                         {
773                             int rowCache = dSdtsRowCache[pNum];
774                             int colCache = dSdtsColCache[pNum];
775                             memset(bigSum->data, 0, sizeof(double) * bigSum->rows);
776                             bigSum->data[colCache] = bCB->data[rowCache];                            
777                             break;
778                         }
779                         default:
780                             // 5) sumVec = beCovB dSdt (1xa)
781                             omxDGEMV(TRUE, 1.0, dSdts[pNum], bCB, 0.0, bigSum);
782                     }
783                     break;
784                 case 1:
785                 {
786                     int rowCache = dAdtsRowCache[pNum];
787                     int colCache = dAdtsColCache[pNum];
788                     double value = bCB->data[rowCache];
789
790                     int len = bCB->rows;
791                     int nrow = ZS->rows;
792                     int ncol = ZS->cols;
793
794                     if (ZS->colMajor) 
795                         for(int offset = 0; offset < len; offset++)
796                             bigSum->data[offset] = ZS->data[offset * nrow + colCache] * value;
797                     else
798                         for(int offset = 0; offset < len; offset++)
799                             bigSum->data[offset] = ZS->data[colCache * ncol + offset] * value;
800
801                     // Term with dSigma/dTheta
802                     //          4) sumVec += beCovB ZS dAdt (1xa)
803                     omxDGEMV(TRUE, 1.0, ZS, bCB, 0.0, tempVec);
804                     bigSum->data[colCache] += tempVec->data[rowCache];
805
806                     if (dSdtsCount[pNum]) {
807                         //  5) sumVec = beCovB dSdt (1xa)
808                         omxDGEMV(TRUE, 1.0, dSdts[pNum], bCB, 1.0, bigSum);
809                     }               
810                     break;
811                 }
812                 default:
813                     //          3) sumVec = beCovB dAdt ZS (1xa)
814                     omxDGEMV(TRUE, 1.0, dAdts[pNum], bCB, 0.0, tempVec);
815                     omxDGEMV(TRUE, 1.0, ZS, tempVec, 0.0, bigSum);
816                     // Term with dSigma/dTheta
817                     //          4) sumVec += beCovB ZS dAdt (1xa)
818                     omxDGEMV(TRUE, 1.0, ZS, bCB, 0.0, tempVec);
819                     omxDGEMV(TRUE, 1.0, dAdts[pNum], tempVec, 1.0, bigSum);
820                     if (dSdtsCount[pNum]) {
821                         //  5) sumVec = beCovB dSdt (1xa)
822                         omxDGEMV(TRUE, 1.0, dSdts[pNum], bCB, 1.0, bigSum);
823                     }
824             }
825  
826             //          6) sumVec *= B^T --> 1xf
827             omxDGEMV(FALSE, 1.0, B, bigSum, 0.0, lilSum);
828
829             // Factorizing dMudt
830             //          7) sumVec += 2 * B dAdt ZM (1xa)
831             //              Note: in the paper, expected manifest means are Bm
832             switch(dAdtsCount[pNum]) {
833                 case 0:
834                     break;
835                 case 1:
836                 {
837                     int rowCache = dAdtsRowCache[pNum];
838                     int colCache = dAdtsColCache[pNum];
839                     double value = ZM->data[colCache];
840
841                     int len = B->rows;
842                     int nrow = B->rows;
843                     int ncol = B->cols;
844
845                     if (B->colMajor) 
846                         for(int offset = 0; offset < len; offset++)
847                             lilSum->data[offset] += 2.0 * B->data[rowCache * nrow + offset] * value;
848                     else
849                         for(int offset = 0; offset < len; offset++)
850                             lilSum->data[offset] += 2.0 * B->data[offset * ncol + rowCache] * value;
851
852                     break;
853                 }
854                 default:
855                     omxDGEMV(FALSE, 1.0, dAdts[pNum], ZM, 0.0, tempVec);
856                     omxDGEMV(FALSE, 2.0, B, tempVec, 1.0, lilSum); // :::DEBUG:::<-- Save B %*% tempVec
857             }
858
859             //          8) sumVec += 2 * B dMdt (1xf)
860             switch(dMdtsCount[pNum]) {
861                 case 0:
862                     break;
863                 case 1: 
864                 {
865                     int moffset, boffset, nrow;
866
867                     moffset = dMdtsColCache[pNum];
868                     nrow = B->rows;
869                     boffset = moffset * nrow;
870
871                     for(int row = 0; row < nrow; row++)
872                         lilSum->data[row] += 2.0 * B->data[boffset + row];
873
874                     break;
875                 }
876                 default:
877                     omxDGEMV(FALSE, 2.0, B, dMdts[pNum], 1.0, lilSum); // :::DEBUG::: <-- Save B %*% dM
878             }
879
880             //          9) sum = sumVec beCov (1x1)
881             int len = lilSum->rows * lilSum->cols;
882             meanInfluence[pNum] = F77_CALL(ddot)(&len, lilSum->data, &onei, beCov->data, &onei);
883         }
884             
885     } else { 
886         for(int i = 0; i < nParam; i++)
887             meanInfluence[i] = 0;
888     }
889     
890     for(int pNum = 0; pNum < nParam; pNum++) {
891         int nextP = pNums[pNum]; // Original varList parameter number
892         result[pNum] = (covInfluence[pNum] * (n-1)) - (meanInfluence[pNum] * n);
893         if(OMX_DEBUG) {
894             Rprintf("Revised calculation for Gradient value %d: Cov: %3.9f, Mean: %3.9f, total: %3.9f\n",
895                 nextP, covInfluence[pNum], meanInfluence[pNum], result[pNum]);
896         }
897     }
898     
899     // Phew.
900
901 }
902
903
904 void calculateRAMGradientComponents(omxExpectation* oo, omxMatrix** dSigmas, omxMatrix** dMus, int* status) {
905
906     // Steps:
907     // 1) (Re) Calculate current A, S, F, and Z (where Z = (I-A)^-1)
908     // 2) E = Z S Z^T
909     // 3) B = F Z
910     // 4) For each location in locations:
911     //      1) Calculate dA/dt, dS/dt, and dM/dt by substituting 1s into empty matrices
912     //      2) C = B dAdt E F^T
913     //      3) dSigma/dT = C + C^T + B dS/dT B^T
914     //      4) dM/dT = B dA/dT Z b + B dM/dT
915
916     omxRAMExpectation* oro = (omxRAMExpectation*)(oo->argStruct);
917                                 // Size reference: A is axa, F is fxf
918     omxMatrix* A = oro->A;      // axa
919     omxMatrix* S = oro->S;      // axa
920     omxMatrix* F = oro->F;      // fxf
921     omxMatrix* Z = oro->Z;      // axa
922     // omxMatrix* I = oro->I;      // axa
923     omxMatrix* M = oro->M;      // 1xf
924     omxMatrix* B = oro->X;      // fxa
925     omxMatrix* U = oro->U;      // fxa
926     omxMatrix* EF = oro->EF;    // fxa
927     omxMatrix* ZM = oro->ZM;    // 1xf
928     omxMatrix* V = oro->V;      // fxa
929     omxMatrix* W = oro->W;      // axa
930         omxMatrix* E = oro->Ax;     // axa
931     
932     omxMatrix* dA = oro->dA;
933     omxMatrix* dS = oro->dS;
934     omxMatrix* dM = oro->dM;
935     
936     // int numIters = oro->numIters;
937     int Amat = A->matrixNumber;
938     int Smat = S->matrixNumber;
939     int Mmat = 0;
940     if(M != NULL) Mmat = M->matrixNumber;
941     
942     omxFreeVar* varList = oo->currentState->freeVarList;
943     int nLocs = oo->currentState->numFreeParams;
944
945     // Assumed.
946     // if(omxNeedsUpdate(Z)) {
947     //     // (Re)Calculate current A, S, F, and Z
948     //     omxCalculateRAMCovarianceAndMeans(A, S, F, M, cov, means, numIters, I, Z, oro->Y, B, Ax);
949     // }
950     
951     // TODO: Handle repeated-call cases.
952     // E = Z S (Z^T)
953     omxDGEMM(FALSE, FALSE, 1.0, Z, S, 0.0, W);
954     omxDGEMM(FALSE, TRUE, 1.0, W, Z, 0.0, E);
955     
956     //EF = E F^T
957     omxDGEMM(FALSE, TRUE, 1.0, E, F, 0.0, EF);
958
959     // B = F Z
960     omxDGEMM(FALSE, FALSE, 1.0, F, Z, 0.0, B);
961     
962     // ZM = ZM
963     if(M != NULL)
964         omxDGEMV(FALSE, 1.0, Z, M, 0.0, ZM);
965     
966     // Step through free params 
967     // Note: This should be parallelized at the next level up.
968     //  This function itself should serially perform one computation for each location
969     //  given in the sequence.  Always write to the appropriate location so that writes
970     //  can be shared across thread-level parallelism.
971     
972     for(int param = 0; param < nLocs; param++) {
973         //  Repeated from above.  For each parameter:
974         //      1) Calculate dA/dt, dS/dt, and dM/dt by substituting 1s into empty matrices
975         
976         omxFreeVar var = varList[param];
977         
978         // Zero dA, dS, and dM.  // TODO: speed
979         for( int k = 0; k < dA->cols; k++) {
980             for( int j = 0; j < dA->rows; j++) {
981                 omxSetMatrixElement(dA, j, k, 0.0);
982                 omxSetMatrixElement(dS, j, k, 0.0);
983             }
984             if(M != NULL) omxSetMatrixElement(dM, 0, k, 0.0);
985         }
986         status[param] = 0;
987
988         // Create dA, dS, and dM mats for each Free Parameter
989         for(int varLoc = 0; varLoc < var.numLocations; varLoc++) {
990             int varMat = ~var.matrices[varLoc]; // Matrices are numbered from ~0 to ~N
991             if(varMat == Amat) {
992                 omxSetMatrixElement(dA, var.row[varLoc], var.col[varLoc], 1);
993             } else if(varMat == Smat) {
994                 omxSetMatrixElement(dS, var.row[varLoc], var.col[varLoc], 1);
995             } else if(varMat == Mmat && M != NULL) {
996                 omxSetMatrixElement(dM, var.row[varLoc], var.col[varLoc], 1);
997             } else {
998                 // This parameter has some outside effect
999                 // We cannot directly estimate its effects on the likelihood
1000                 // For now, skip.
1001                 // TODO: Find a way to deal with these situations
1002                 status[param] = -1;
1003                 if(OMX_DEBUG) { Rprintf("Skipping parameter %d because %dth element has outside influence %d. Looking for %d, %d, or %d.\n", param, varLoc, varMat, Amat, Smat, Mmat);}
1004                 break;
1005             }
1006         }
1007         
1008         if(status[param] < 0) {
1009             continue;  // Skip this one.
1010         }
1011         
1012         omxMatrix* C = dSigmas[param];
1013         omxMatrix* dMu = dMus[param];
1014         
1015         //      2) C = B dAdt EF
1016         omxDGEMM(FALSE, FALSE, 1.0, B, dA, 0.0, U);
1017         omxDGEMM(FALSE, FALSE, 1.0, U, EF, 0.0, C);
1018         
1019         // Note: U is saved here for calculation of M, below.
1020         //      3) dSigma/dT = C + C^T + B dS/dT B^T
1021         omxAddOwnTranspose(&C, 0, C);
1022         omxDGEMM(FALSE, FALSE, 1.0, B, dS, 0.0, V);
1023         omxDGEMM(FALSE, TRUE,  1.0, V, B, 1.0, C);
1024         
1025         if(M != NULL) {
1026             //      4) dM/dT = B dA/dT Z M + B dM/dT
1027             omxDGEMV(FALSE, 1.0, U, ZM, 0.0, dMu);
1028             omxDGEMV(FALSE, 1.0, B, dM, 1.0, dMu);
1029
1030         }
1031
1032     }
1033
1034 }
1035
1036 omxMatrix* omxGetRAMExpectationComponent(omxExpectation* ox, omxFitFunction* off, const char* component) {
1037         
1038         if(OMX_DEBUG) { Rprintf("RAM expectation: %s requested--", component); }
1039
1040         omxRAMExpectation* ore = (omxRAMExpectation*)(ox->argStruct);
1041         omxMatrix* retval = NULL;
1042
1043         if(!strncmp("cov", component, 3)) {
1044                 retval = ore->cov;
1045         } else if(!strncmp("mean", component, 4)) {
1046                 retval = ore->means;
1047         } else if(!strncmp("pvec", component, 4)) {
1048                 // Once implemented, change compute function and return pvec
1049         }
1050         
1051         if(OMX_DEBUG) { Rprintf("Returning 0x%x.\n", retval); }
1052
1053         return retval;
1054
1055 }