Lines Matching refs:P
112 const mat<TYPE, C, C>& P) { in scaleCovariance() argument
119 double v(A[c][r]*P[c][c]*0.5); in scaleCovariance()
121 v += A[k][r] * P[c][k]; in scaleCovariance()
252 P = 0; in initFusion()
397 if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) || in checkState()
398 !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) { in checkState()
400 P = 0; in checkState()
490 P = Phi*P*transpose(Phi) + GQGt; in predict()
509 const mat33_t S(scaleCovariance(L, P[0][0]) + R); in update()
512 K[0] = P[0][0] * LtSi; in update()
513 K[1] = transpose(P[1][0])*LtSi; in update()
524 P[0][0] -= K0L*P[0][0]; in update()
525 P[1][1] -= K1L*P[1][0]; in update()
526 P[1][0] -= K0L*P[1][0]; in update()
527 P[0][1] = transpose(P[1][0]); in update()