class KalmanVectorFilter {

    public RealVector step(RealVector observation, int time) {
        // Create g = [t^2 / 2, t]^T matrix
        RealVector g = new ArrayRealVector(2);
        g.setEntry(0, 0.5 * time * time);
        g.setEntry(1, time);
        RealMatrix Q = g.outerProduct(g).scalarMultiply(qScale);
        RealMatrix R = MatrixUtils.createRealIdentityMatrix(1).scalarMultiply(rScale);
        R = R.scalarMultiply(time * time);

        RealMatrix F = MatrixUtils.createRealIdentityMatrix(2);
        F.setEntry(0, 1, time);

        RealMatrix priorNextState = F.multiply(state);
        RealMatrix priorNextCov = F.multiply(cov).multiply(F.transpose()).add(Q); // F * cov * F^T + Q

        RealVector measurementResidual = observation.subtract(measure(priorNextState)); // row vector
        RealMatrix residualCovariance = H.multiply(priorNextCov).multiply(H.transpose()).add(R);
        RealMatrix kalmanGain = priorNextCov.multiply(H.transpose()).multiply(AlgebraUtils.invertMatrix(residualCovariance));
        // kalmanGain should be a 1x2 matrix
        state = priorNextState.add(kalmanGain.getColumnVector(0).outerProduct(measurementResidual));
        cov = MatrixUtils.createRealIdentityMatrix(2).subtract(kalmanGain.multiply(H)).multiply(priorNextCov);

        return measure(state);
    }

    public  KalmanVectorFilter(RealVector startLoc, double qScale);
    public  KalmanVectorFilter(RealVector startLoc, double qScale, double rScale);

}