Lines Matching refs:P
95 const mat<TYPE, C, C>& P) { in scaleCovariance() argument
102 double v(A[c][r]*P[c][c]*0.5); in scaleCovariance()
104 v += A[k][r] * P[c][k]; in scaleCovariance()
222 P = 0; in initFusion()
339 if (!isPositiveSemidefinite(P[0][0], SYMMETRY_TOLERANCE) || in checkState()
340 !isPositiveSemidefinite(P[1][1], SYMMETRY_TOLERANCE)) { in checkState()
342 P = 0; in checkState()
390 P = Phi*P*transpose(Phi) + GQGt; in predict()
409 const mat33_t S(scaleCovariance(L, P[0][0]) + R); in update()
412 K[0] = P[0][0] * LtSi; in update()
413 K[1] = transpose(P[1][0])*LtSi; in update()
419 P[0][0] -= K0L*P[0][0]; in update()
420 P[1][1] -= K1L*P[1][0]; in update()
421 P[1][0] -= K0L*P[1][0]; in update()
422 P[0][1] = transpose(P[1][0]); in update()