/**
* Updates the state provided the observation from a sensor.
*
* @param z Measurement.
*/
public void update(double[] z)
{
if(skipIdenticalMeasurements)
{
bool areIdentical = true;
for(int i = 0; i<z.Length; ++i)
{
if(z[i] != previousMeasurements[i])
{
areIdentical = false;
previousMeasurements[i] = z[i];
}
}
if(areIdentical && identicalMeasurementsCount < identicalMeasurementsCap)
{
++identicalMeasurementsCount;
return;
}
else
identicalMeasurementsCount = 0;
}
// y = z - H x
//mult(H,x,y);
//for(int i=0; i<u.numRows; ++i)
// u.set(i, (double) z[i]);
//sub(u,y,y);
u = new Matrix(z);
y = u - (H*x);
y = y.Re();
// S = H P H' + R
//S_inv=S.copy();
//mult(H,P,c);
//multTransB(c,H,S);
//addEquals(S,S_inv);
S = ((H*P)*H.Transpose()) + R;
S = S.Re();
// K = PH'S^(-1)
//if( !invert(S,S_inv) ) throw new RuntimeException("Invert failed");
//multTransA(H,S_inv,d);
//mult(P,d,K);
try
{
S_inv = S.Inverse();
}
catch(Exception e)
{
Debug.Log("KalmanFilter.cs: Failed to invert S matrix.\n" + e);
}
K = P*(H.Transpose()*S_inv);
K = K.Re();
// x = x + Ky
//mult(K,y,a);
//addEquals(x,a);
x = x + (K*y);
// P = (I-kH)P = P - (KH)P = P-K(HP)
//mult(H,P,c);
//mult(K,c,b);
//subEquals(P,b);
P = P - (K*(H*P));
P = P.Re();
}
/**
* Predicts the state of the system forward one time step.
*/
public void predict() {
// x = F x
x = F*x;
x = x.Re();
//mult(F,x,a);
//x.set(a);
// P = F P F' + Q
P = ((F*P)*F.Transpose()) + Q;
P = P.Re();
//mult(F,P,b);
//multTransB(b,F, P);
//addEquals(P,Q);
}