List of usage examples for org.apache.commons.math3.filter DefaultMeasurementModel DefaultMeasurementModel
public DefaultMeasurementModel(final RealMatrix measMatrix, final RealMatrix measNoise)
From source file:net.liuxuan.temp.filterTest.java
public static void main(String[] args) { double constantVoltage = 10d; double measurementNoise = 0.1d; double processNoise = 1e-5d; // A = [ 1 ]//from w ww . j a va 2s. c o m RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d }); // B = null RealMatrix B = null; // H = [ 1 ] RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d }); // x = [ 10 ] RealVector x = new ArrayRealVector(new double[] { constantVoltage }); // Q = [ 1e-5 ] RealMatrix Q = new Array2DRowRealMatrix(new double[] { processNoise }); // P = [ 1 ] RealMatrix P0 = new Array2DRowRealMatrix(new double[] { 1d }); // R = [ 0.1 ] RealMatrix R = new Array2DRowRealMatrix(new double[] { measurementNoise }); ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0); MeasurementModel mm = new DefaultMeasurementModel(H, R); KalmanFilter filter = new KalmanFilter(pm, mm); // process and measurement noise vectors RealVector pNoise = new ArrayRealVector(1); RealVector mNoise = new ArrayRealVector(1); RandomGenerator rand = new JDKRandomGenerator(); // iterate 60 steps for (int i = 0; i < 60; i++) { filter.predict(); // simulate the process // pNoise.setEntry(0, processNoise * rand.nextGaussian()); pNoise.setEntry(0, Math.sin(Math.PI * 2 * i / 6)); // System.out.println("============"); // System.out.println(Math.sin(Math.PI*2*i/6)); // x = A * x + p_noise x = A.operate(x).add(pNoise); // simulate the measurement // mNoise.setEntry(0, measurementNoise * rand.nextGaussian()); mNoise.setEntry(0, 0); // z = H * x + m_noise RealVector z = H.operate(x).add(mNoise); filter.correct(z); double voltage = filter.getStateEstimation()[0]; System.out.println(voltage); // state estimate shouldn't be larger than the measurement noise double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]); System.out.println("diff = " + diff); } }
From source file:net.liuxuan.device.w3330.KalmanFilterHolder.java
private void initfilter() { pm = new DefaultProcessModel(A, B, Q, x, P0); mm = new DefaultMeasurementModel(H, R); filter = new KalmanFilter(pm, mm); // z = H.operate(x).add(mNoise); }
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 w w w . j ava 2 s . c o m*/ // 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:eu.qualimaster.monitoring.profiling.predictors.Kalman.java
/** * Update models and the Kalman-Filter after a change in the parameters / matrices. *///from w w w . java 2s .com private void reinitialize() { pm = new DefaultProcessModel(mA, mB, mQ, xVector, mP); mm = new DefaultMeasurementModel(mH, mR); filter = new KalmanFilter(pm, mm); }