Example usage for org.apache.commons.math3.filter KalmanFilter getStateDimension

Introduction

In this page you can find the example usage for org.apache.commons.math3.filter KalmanFilter getStateDimension.

Prototype

public int getStateDimension() 

Source Link

Document

Returns the dimension of the state estimation vector.

Usage

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 v  a2s .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 ww. j av  a  2  s.  c o m
    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);
}

---