//@Override
public void update(DMatrixRMaj z, DMatrixRMaj R)
{
// y = z - H x
CommonOps_DDRM.mult(H, x, y);
CommonOps_DDRM.subtract(z, y, y);
// S = H P H' + R
CommonOps_DDRM.mult(H, P, c);
CommonOps_DDRM.multTransB(c, H, S);
CommonOps_DDRM.addEquals(S, R);
// K = PH'S^(-1)
if (!solver.setA(S))
{
throw new InvalidOperationException("Invert failed");
}
solver.invert(S_inv);
CommonOps_DDRM.multTransA(H, S_inv, d);
CommonOps_DDRM.mult(P, d, K);
// x = x + Ky
CommonOps_DDRM.mult(K, y, a);
CommonOps_DDRM.addEquals(x, a);
// P = (I-kH)P = P - (KH)P = P-K(HP)
CommonOps_DDRM.mult(H, P, c);
CommonOps_DDRM.mult(K, c, b);
CommonOps_DDRM.subtractEquals(P, b);
}
/// <summary>
/// Performs a matrix addition and scale operation.
/// <code>c = a + beta*b</code>
/// where c is the returned matrix, a is this matrix, and b is the passed in matrix.
/// </summary>
/// <see cref="CommonOps_DDRM.add(DMatrixD1, double, DMatrixD1, DMatrixD1)"/>
public override SimpleMatrixD plus(double beta, SimpleMatrixD b)
{
SimpleMatrixD ret = copy();
var rm = ret.getMatrix();
var bm = b.getMatrix();
CommonOps_DDRM.addEquals(rm, beta, bm);
return(ret);
}
//@Override
public void predict()
{
// x = F x
CommonOps_DDRM.mult(F, x, a);
x.set(a);
// P = F P F' + Q
CommonOps_DDRM.mult(F, P, b);
CommonOps_DDRM.multTransB(b, F, P);
CommonOps_DDRM.addEquals(P, Q);
}
