//@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); }