public KalmanFilter()
{
pX = &Xa;
pXprev = &Xb;
pP = &Pa;
pPprev = &Pb;
Xa.zero();
Xb.zero();
Z.zero();
F.zero();
F[0][1] = 1.0f;
F[1][2] = -1.0f;
Phi.identity(); // Depends on time
PhiTranspose.identity(); // Depends on time
G.zero();
G[1][0] = 1.0f;
G[2][1] = 1.0f;
GTranspose.zero();
GTranspose[0][1] = 1.0f;
GTranspose[1][2] = 1.0f;
H.zero();
H[0][0] = 1.0f;
HTranspose.zero();
HTranspose[0][0] = 1.0f;
K.zero(); // gain
// The above will not change for our filter. For a general filter they will.
// These are the intial covariance estimates, feel free to muck with them.
Pa.zero();
Pa[0][0] = 0.05f * 0.05f;
Pa[1][1] = 0.5f * 0.5f;
Pa[2][2] = 10.0f * 10.0f;
Pb = Pa;
// The following will change to appropriate values for noise.
timePropNoiseVector[0] = 0.01f; // m / s^2
timePropNoiseVector[1] = 0.10f; // m / s^2 / s
measurmentNoiseVector[0] = 0.005f; // =1/2 cm;
}
