KalmanVectorFilter { public RealVector step(RealVector observation, int time) { 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); RealVector measurementResidual = observation.subtract(measure(priorNextState)); RealMatrix residualCovariance = H.multiply(priorNextCov).multiply(H.transpose()).add(R); RealMatrix kalmanGain = priorNextCov.multiply(H.transpose()).multiply(AlgebraUtils.invertMatrix(residualCovariance)); state = priorNextState.add(kalmanGain.getColumnVector(0).outerProduct(measurementResidual)); cov = MatrixUtils.createRealIdentityMatrix(2).subtract(kalmanGain.multiply(H)).multiply(priorNextCov); return measure(state); } KalmanVectorFilter(RealVector startLoc, double qScale);  KalmanVectorFilter(RealVector startLoc, double qScale, double rScale); void reset(RealVector startLoc); RealVector step(RealVector observation, int time);  }
@Test public void runTest() throws Exception { MacroBaseConf conf = new MacroBaseConf() .set(MacroBaseConf.RANDOM_SEED, 4) .set(MacroBaseConf.DATA_LOADER_TYPE, "CSV_LOADER") .set(MacroBaseConf.CSV_COMPRESSION, CSVIngester.Compression.GZIP) .set(MacroBaseConf.CSV_INPUT_FILE, "src/test/resources/data/2gaussians-500points.csv.gz") .set(MacroBaseConf.METRICS, "XX, YY") .set(MacroBaseConf.ATTRIBUTES, ""); List<Datum> data = Drainer.drainIngest(conf); assertEquals(500, data.size()); KalmanVectorFilter f = new KalmanVectorFilter(new ArrayRealVector(2), 1e-6, 1.); List<Datum> oneCluster = data.subList(201, 500); List<RealVector> filtered = oneCluster.stream().map(d -> f.step(d.metrics(), 1)).collect(Collectors.toList()); List<Datum> unfilteredlast10 = oneCluster.subList(oneCluster.size() - 10, oneCluster.size()); List<RealVector> filteredlast10 = filtered.subList(filtered.size() - 10, filtered.size()); double filteredMax = filteredlast10.stream().mapToDouble(d -> d.getEntry(0)).max().getAsDouble(); double filteredMin = filteredlast10.stream().mapToDouble(d -> d.getEntry(0)).min().getAsDouble(); double unfilteredMax = unfilteredlast10.stream().mapToDouble(d -> d.metrics().getEntry(0)).max().getAsDouble(); double unfilteredMin = unfilteredlast10.stream().mapToDouble(d -> d.metrics().getEntry(0)).min().getAsDouble(); assert filteredMax < unfilteredMax; assert filteredMin > unfilteredMin; }