222         initZeroMatrix(&fusion->P[0][0]);  in fusion_init_complete()
223         initZeroMatrix(&fusion->P[0][1]);  in fusion_init_complete()
224         initZeroMatrix(&fusion->P[1][0]);  in fusion_init_complete()
225         initZeroMatrix(&fusion->P[1][1]);  in fusion_init_complete()
247     if (!mat33IsPositiveSemidefinite(&fusion->P[0][0], SYMMETRY_TOLERANCE)  in fusionCheckState()
249                 &fusion->P[1][1], SYMMETRY_TOLERANCE)) {  in fusionCheckState()
251         initZeroMatrix(&fusion->P[0][0]);  in fusionCheckState()
252         initZeroMatrix(&fusion->P[0][1]);  in fusionCheckState()
253         initZeroMatrix(&fusion->P[1][0]);  in fusionCheckState()
254         initZeroMatrix(&fusion->P[1][1]);  in fusionCheckState()
356     mat33Multiply(&Pnew[0][0], &fusion->Phi0[0], &fusion->P[0][0]);  in fusionPredict()
357     mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][0]);  in fusionPredict()
360     mat33Multiply(&Pnew[0][1], &fusion->Phi0[0], &fusion->P[0][1]);  in fusionPredict()
361     mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][1]);  in fusionPredict()
364     Pnew[1][0] = fusion->P[1][0];  in fusionPredict()
365     Pnew[1][1] = fusion->P[1][1];  in fusionPredict()
369     mat33MultiplyTransposed2(&fusion->P[0][0], &Pnew[0][0], &fusion->Phi0[0]);  in fusionPredict()
371     mat33Add(&fusion->P[0][0], &tmp);  in fusionPredict()
373     fusion->P[0][1] = Pnew[0][1];  in fusionPredict()
375     mat33MultiplyTransposed2(&fusion->P[1][0], &Pnew[1][0], &fusion->Phi0[0]);  in fusionPredict()
377     mat33Add(&fusion->P[1][0], &tmp);  in fusionPredict()
379     fusion->P[1][1] = Pnew[1][1];  in fusionPredict()
381     mat33Add(&fusion->P[0][0], &fusion->GQGt[0][0]);  in fusionPredict()
382     mat33Add(&fusion->P[0][1], &fusion->GQGt[0][1]);  in fusionPredict()
383     mat33Add(&fusion->P[1][0], &fusion->GQGt[1][0]);  in fusionPredict()
384     mat33Add(&fusion->P[1][1], &fusion->GQGt[1][1]);  in fusionPredict()
400 static void scaleCovariance(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *P) {  in scaleCovariance()  argument
408                 float v = A->elem[c][r] * P->elem[c][c] * 0.5f;  in scaleCovariance()
411                     v += A->elem[k][r] * P->elem[c][k];  in scaleCovariance()
445     scaleCovariance(&S, &L, &fusion->P[0][0]);  in fusionUpdate()
456     mat33Multiply(&K[0], &fusion->P[0][0], &LtSi);  in fusionUpdate()
457     mat33MultiplyTransposed(&K[1], &fusion->P[0][1], &LtSi);  in fusionUpdate()
466     mat33Multiply(&tmp, &K0L, &fusion->P[0][0]);  in fusionUpdate()
467     mat33Sub(&fusion->P[0][0], &tmp);  in fusionUpdate()
469     mat33Multiply(&tmp, &K1L, &fusion->P[0][1]);  in fusionUpdate()
470     mat33Sub(&fusion->P[1][1], &tmp);  in fusionUpdate()
472     mat33Multiply(&tmp, &K0L, &fusion->P[0][1]);  in fusionUpdate()
473     mat33Sub(&fusion->P[0][1], &tmp);  in fusionUpdate()
475     mat33Transpose(&fusion->P[1][0], &fusion->P[0][1]);  in fusionUpdate()