Java Code Examples for org.apache.commons.math3.linear.RealVector#mapMultiply()
The following examples show how to use
org.apache.commons.math3.linear.RealVector#mapMultiply() .
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example. You may check out the related API usage on the sidebar.
Example 1
| Source File: KalmanFilterTest.java From astor with GNU General Public License v2.0 | 4 votes |
|
@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 B = 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
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, 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());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// 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++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
double mNoise = measurementNoise * rand.nextGaussian();
// z = H * x + m_noise
RealVector z = H.operate(x).mapAdd(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = FastMath.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
}
// error covariance of the velocity should be already very low (< 0.1)
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
0.1d, 1e-6) < 0);
}
Example 2
| Source File: KalmanFilterTest.java From astor with GNU General Public License v2.0 | 4 votes |
|
@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 B = new Array2DRowRealMatrix(
new double[][] { { Math.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[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
{ Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });
// Q = [ dt^4/4 dt^3/2 ]
// [ dt^3/2 dt^2 ]
RealMatrix Q = tmp.scalarMultiply(Math.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[] { Math.pow(measurementNoise, 2) });
// constant control input, increase velocity by 0.1 m/s per cycle
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, 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());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// 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[] { Math.pow(dt, 2d) / 2d, dt });
RealVector mNoise = new ArrayRealVector(1);
// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
mNoise.setEntry(0, measurementNoise * rand.nextGaussian());
// z = H * x + m_noise
RealVector z = H.operate(x).add(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
}
// error covariance of the velocity should be already very low (< 0.1)
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
0.1d, 1e-6) < 0);
}
Example 3
| Source File: KalmanFilterTest.java From astor with GNU General Public License v2.0 | 4 votes |
|
@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 B = new Array2DRowRealMatrix(
new double[][] { { Math.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[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
{ Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });
// Q = [ dt^4/4 dt^3/2 ]
// [ dt^3/2 dt^2 ]
RealMatrix Q = tmp.scalarMultiply(Math.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[] { Math.pow(measurementNoise, 2) });
// constant control input, increase velocity by 0.1 m/s per cycle
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, 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());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// 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[] { Math.pow(dt, 2d) / 2d, dt });
RealVector mNoise = new ArrayRealVector(1);
// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
mNoise.setEntry(0, measurementNoise * rand.nextGaussian());
// z = H * x + m_noise
RealVector z = H.operate(x).add(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
}
// error covariance of the velocity should be already very low (< 0.1)
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
0.1d, 1e-6) < 0);
}
Example 4
| Source File: KalmanFilterTest.java From astor with GNU General Public License v2.0 | 4 votes |
|
@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 B = new Array2DRowRealMatrix(
new double[][] { { Math.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[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
{ Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });
// Q = [ dt^4/4 dt^3/2 ]
// [ dt^3/2 dt^2 ]
RealMatrix Q = tmp.scalarMultiply(Math.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[] { Math.pow(measurementNoise, 2) });
// constant control input, increase velocity by 0.1 m/s per cycle
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, 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());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// 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[] { Math.pow(dt, 2d) / 2d, dt });
RealVector mNoise = new ArrayRealVector(1);
// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
mNoise.setEntry(0, measurementNoise * rand.nextGaussian());
// z = H * x + m_noise
RealVector z = H.operate(x).add(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
}
// error covariance of the velocity should be already very low (< 0.1)
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
0.1d, 1e-6) < 0);
}
Example 5
| Source File: KalmanFilterTest.java From astor with GNU General Public License v2.0 | 4 votes |
|
@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 B = new Array2DRowRealMatrix(
new double[][] { { Math.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[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
{ Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });
// Q = [ dt^4/4 dt^3/2 ]
// [ dt^3/2 dt^2 ]
RealMatrix Q = tmp.scalarMultiply(Math.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[] { Math.pow(measurementNoise, 2) });
// constant control input, increase velocity by 0.1 m/s per cycle
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, 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());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// 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[] { Math.pow(dt, 2d) / 2d, dt });
RealVector mNoise = new ArrayRealVector(1);
// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
mNoise.setEntry(0, measurementNoise * rand.nextGaussian());
// z = H * x + m_noise
RealVector z = H.operate(x).add(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
}
// error covariance of the velocity should be already very low (< 0.1)
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
0.1d, 1e-6) < 0);
}
Example 6
| Source File: KalmanFilterTest.java From astor with GNU General Public License v2.0 | 4 votes |
|
@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 B = new Array2DRowRealMatrix(
new double[][] { { Math.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[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
{ Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });
// Q = [ dt^4/4 dt^3/2 ]
// [ dt^3/2 dt^2 ]
RealMatrix Q = tmp.scalarMultiply(Math.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[] { Math.pow(measurementNoise, 2) });
// constant control input, increase velocity by 0.1 m/s per cycle
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, 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());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// 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[] { Math.pow(dt, 2d) / 2d, dt });
RealVector mNoise = new ArrayRealVector(1);
// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
mNoise.setEntry(0, measurementNoise * rand.nextGaussian());
// z = H * x + m_noise
RealVector z = H.operate(x).add(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
}
// error covariance of the velocity should be already very low (< 0.1)
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
0.1d, 1e-6) < 0);
}
Example 7
| Source File: KalmanFilterTest.java From astor with GNU General Public License v2.0 | 4 votes |
|
@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 B = new Array2DRowRealMatrix(
new double[][] { { Math.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[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
{ Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });
// Q = [ dt^4/4 dt^3/2 ]
// [ dt^3/2 dt^2 ]
RealMatrix Q = tmp.scalarMultiply(Math.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[] { Math.pow(measurementNoise, 2) });
// constant control input, increase velocity by 0.1 m/s per cycle
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, 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());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// 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[] { Math.pow(dt, 2d) / 2d, dt });
RealVector mNoise = new ArrayRealVector(1);
// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
mNoise.setEntry(0, measurementNoise * rand.nextGaussian());
// z = H * x + m_noise
RealVector z = H.operate(x).add(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
}
// error covariance of the velocity should be already very low (< 0.1)
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
0.1d, 1e-6) < 0);
}
Example 8
| Source File: KalmanFilterTest.java From astor with GNU General Public License v2.0 | 4 votes |
|
@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 B = 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
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, 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());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// 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++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
double mNoise = measurementNoise * rand.nextGaussian();
// z = H * x + m_noise
RealVector z = H.operate(x).mapAdd(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = FastMath.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
}
// error covariance of the velocity should be already very low (< 0.1)
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
0.1d, 1e-6) < 0);
}
