private void Update()
{
//Update K
//K = P*Ht/(H*P*Ht + R);
MatrixS Ht = MatrixS.Transpose(H);
MatrixS PHt = P * Ht;
MatrixS HPHt = H * P * Ht;
MatrixS HPHtR = HPHt + R;
MatrixS HRInv = HPHtR.Invert();
MatrixS K = PHt * HRInv;
//Update X
//Kalman Gain
//measurement update correction
//X = X0 + K*(D(data) - H*X0);
MatrixS HX0 = H * X0;
MatrixS DHX0 = D - HX0;
MatrixS KDHX0 = K * DHX0;
X = X0 + KDHX0;
//zdmzc z
MatrixS KH = K * H;
MatrixS IMinus = I - KH;
P = IMinus * P;
}
private void Predict()
{
//X0 = F * X;
//P' = F * P * F + Q;
if (iIndex++ == 0)
{
X[0, 0] = D[0, 0];
}
X0 = F * X;
MatrixS Ft = MatrixS.Transpose(F);
MatrixS FPFt = F * P * Ft;
P = FPFt + Q;
}