List of usage examples for org.apache.commons.math3.filter KalmanFilter getMeasurementDimension
public int getMeasurementDimension()
From source file:com.winterwell.maths.stats.algorithms.KalmanFilterTest.java
@Test public void testConstantAcceleration() { // simulates a vehicle, accelerating at a constant rate (0.1 m/s) // discrete time interval double dt = 0.1d; // position measurement noise (meter) double measurementNoise = 10d; // acceleration noise (meter/sec^2) double accelNoise = 0.2d; // A = [ 1 dt ] // [ 0 1 ] RealMatrix A = new Array2DRowRealMatrix(new double[][] { { 1, dt }, { 0, 1 } }); // B = [ dt^2/2 ] // [ dt ] RealMatrix Bnull = new Array2DRowRealMatrix(new double[][] { { FastMath.pow(dt, 2d) / 2d }, { dt } }); // H = [ 1 0 ] RealMatrix H = new Array2DRowRealMatrix(new double[][] { { 1d, 0d } }); // x = [ 0 0 ] RealVector x = new ArrayRealVector(new double[] { 0, 0 }); RealMatrix tmp = new Array2DRowRealMatrix( new double[][] { { FastMath.pow(dt, 4d) / 4d, FastMath.pow(dt, 3d) / 2d }, { FastMath.pow(dt, 3d) / 2d, FastMath.pow(dt, 2d) } }); // Q = [ dt^4/4 dt^3/2 ] // [ dt^3/2 dt^2 ] RealMatrix Q = tmp.scalarMultiply(FastMath.pow(accelNoise, 2)); // P0 = [ 1 1 ] // [ 1 1 ] RealMatrix P0 = new Array2DRowRealMatrix(new double[][] { { 1, 1 }, { 1, 1 } }); // R = [ measurementNoise^2 ] RealMatrix R = new Array2DRowRealMatrix(new double[] { FastMath.pow(measurementNoise, 2) }); // constant control input, increase velocity by 0.1 m/s per cycle double uv = 0.1d * dt; RealVector u = new ArrayRealVector(new double[] { uv * uv / 2, uv }); ProcessModel pm = new DefaultProcessModel(A, Bnull, Q, x, P0); MeasurementModel mm = new DefaultMeasurementModel(H, R); KalmanFilter filter = new KalmanFilter(pm, mm); Assert.assertEquals(1, filter.getMeasurementDimension()); Assert.assertEquals(2, filter.getStateDimension()); Gaussian state = new Gaussian(mtj(x), mtj(P0)); MatrixUtils.equals(mtj(P0), state.getCovar()); // check the initial state double[] expectedInitialState = new double[] { 0.0, 0.0 }; // assertVectorEquals(expectedInitialState, filter.getStateEstimation()); RandomGenerator rand = new JDKRandomGenerator(); RealVector tmpPNoise = new ArrayRealVector(new double[] { FastMath.pow(dt, 2d) / 2d, dt }); // iterate 60 steps for (int i = 0; i < 60; i++) { state = filter.predict(state, mtj(u)); // Simulate the process RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian()); // x = A * x + B * u + pNoise x = A.operate(x).add(u).add(pNoise); // Simulate the measurement double mNoise = measurementNoise * rand.nextGaussian(); // z = H * x + m_noise RealVector z = H.operate(x).mapAdd(mNoise); state = filter.correct(state, mtj(z)); // state estimate shouldn't be larger than the measurement noise double diff = FastMath.abs(x.getEntry(0) - state.getMean().get(0)); Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0); }/*from www . j a va 2s. c om*/ // error covariance of the velocity should be already very low (< 0.1) Assert.assertTrue(Precision.compareTo(state.getCovar().get(1, 1), 0.1d, 1e-6) < 0); }
From source file:com.winterwell.maths.stats.algorithms.KalmanFilterTest.java
@Test public void testConstant() { // simulates a simple process with a constant state and no control input double constantValue = 10d; double measurementNoise = 0.1d; double processNoise = 1e-5d; // A = [ 1 ]//from w w w. j a va 2 s. c om Matrix A = new Matrix1D(1); // no control input RealMatrix B = null; // H = [ 1 ] Matrix H = new Matrix1D(1); // x = [ 10 ] Vector x = new X(constantValue); // Q = [ 1e-5 ] Matrix Q = new Matrix1D(processNoise); // R = [ 0.1 ] Matrix R = new Matrix1D(measurementNoise); // ProcessModel pm // = new DefaultProcessModel(A, B, Q, // new ArrayRealVector(new double[] { constantValue }), null); // MeasurementModel mm = new DefaultMeasurementModel(H, R); KalmanFilter filter = new KalmanFilter(1, 1); filter.setTransitionMatrix(A); filter.setTransitionNoise(Q); filter.setEmissionMatrix(H); filter.setEmissionNoise(R); Assert.assertEquals(1, filter.getMeasurementDimension()); Assert.assertEquals(1, filter.getStateDimension()); MatrixUtils.equals(Q, filter.getTransitionNoise()); // // check the initial state // double[] expectedInitialState = new double[] { constantValue }; // assertVectorEquals(expectedInitialState, filter.getStateEstimation()); Gaussian hiddenState = new Gaussian(new X(constantValue), new Matrix1D(0)); Vector pNoise = new X(0); Vector mNoise = new X(0); RandomGenerator rand = new JDKRandomGenerator(); // iterate 60 steps for (int i = 0; i < 60; i++) { hiddenState = filter.predict(hiddenState, null); // Simulate the process pNoise.set(0, processNoise * rand.nextGaussian()); // x = A * x + p_noise x = MatrixUtils.apply(A, x).add(pNoise); // Simulate the measurement mNoise.set(0, measurementNoise * rand.nextGaussian()); // z = H * x + m_noise Vector z = MatrixUtils.apply(H, x).add(mNoise); hiddenState = filter.correct(hiddenState, z); // state estimate shouldn't be larger than measurement noise double diff = FastMath.abs(constantValue - hiddenState.getMean().get(0)); // System.out.println(diff); Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0); } // error covariance should be already very low (< 0.02) Assert.assertTrue(Precision.compareTo(hiddenState.getCovar().get(0, 0), 0.02d, 1e-6) < 0); }