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;
}
public static MatrixS Power(MatrixS m, int pow) // Power matrix to exponent
{
if (pow == 0)
{
return(IdentityMatrix(m.rows, m.cols));
}
if (pow == 1)
{
return(m.Duplicate());
}
if (pow == -1)
{
return(m.Invert());
}
MatrixS x;
if (pow < 0)
{
x = m.Invert();
pow *= -1;
}
else
{
x = m.Duplicate();
}
MatrixS ret = IdentityMatrix(m.rows, m.cols);
while (pow != 0)
{
if ((pow & 1) == 1)
{
ret *= x;
}
x *= x;
pow >>= 1;
}
return(ret);
}