[MATH-1062] Use MatrixUtils.inverse to invert a matrix in the KalmanFilter, added new unit test.
git-svn-id: https://svn.apache.org/repos/asf/commons/proper/math/trunk@1539676 13f79535-47bb-0310-9956-ffa450edef68
This commit is contained in:
Thomas Neidhart
parent
4ebd967c96
commit
0aa89cc22e
|
|
@ -51,6 +51,10 @@ If the output is not quite correct, check for invisible trailing spaces!
|
||||||
</properties>
|
</properties>
|
||||||
<body>
|
<body>
|
||||||
<release version="3.3" date="TBD" description="TBD">
|
<release version="3.3" date="TBD" description="TBD">
|
||||||
|
<action dev="tn" type="fix" issue="MATH-1062">
|
||||||
|
A call to "KalmanFilter#correct(...)" may have resulted in "NonSymmetricMatrixException"
|
||||||
|
as the internally used matrix inversion method was using a too strict symmetry check.
|
||||||
|
</action>
|
||||||
<action dev="erans" type="fix" issue="MATH-1058" due-to="Sean Owen">
|
<action dev="erans" type="fix" issue="MATH-1058" due-to="Sean Owen">
|
||||||
Precision improvements (for small values of the argument) in "Beta" function
|
Precision improvements (for small values of the argument) in "Beta" function
|
||||||
and in "LogNormalDistribution" and "WeibullDistribution".
|
and in "LogNormalDistribution" and "WeibullDistribution".
|
||||||
|
|
|
||||||
|
|
@ -20,8 +20,6 @@ import org.apache.commons.math3.exception.DimensionMismatchException;
|
||||||
import org.apache.commons.math3.exception.NullArgumentException;
|
import org.apache.commons.math3.exception.NullArgumentException;
|
||||||
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
|
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
|
||||||
import org.apache.commons.math3.linear.ArrayRealVector;
|
import org.apache.commons.math3.linear.ArrayRealVector;
|
||||||
import org.apache.commons.math3.linear.CholeskyDecomposition;
|
|
||||||
import org.apache.commons.math3.linear.DecompositionSolver;
|
|
||||||
import org.apache.commons.math3.linear.MatrixDimensionMismatchException;
|
import org.apache.commons.math3.linear.MatrixDimensionMismatchException;
|
||||||
import org.apache.commons.math3.linear.MatrixUtils;
|
import org.apache.commons.math3.linear.MatrixUtils;
|
||||||
import org.apache.commons.math3.linear.NonSquareMatrixException;
|
import org.apache.commons.math3.linear.NonSquareMatrixException;
|
||||||
|
|
@ -283,7 +281,7 @@ public class KalmanFilter {
|
||||||
* if the dimension of the control vector does not fit
|
* if the dimension of the control vector does not fit
|
||||||
*/
|
*/
|
||||||
public void predict(final double[] u) throws DimensionMismatchException {
|
public void predict(final double[] u) throws DimensionMismatchException {
|
||||||
predict(new ArrayRealVector(u));
|
predict(new ArrayRealVector(u, false));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -332,7 +330,7 @@ public class KalmanFilter {
|
||||||
*/
|
*/
|
||||||
public void correct(final double[] z)
|
public void correct(final double[] z)
|
||||||
throws NullArgumentException, DimensionMismatchException, SingularMatrixException {
|
throws NullArgumentException, DimensionMismatchException, SingularMatrixException {
|
||||||
correct(new ArrayRealVector(z));
|
correct(new ArrayRealVector(z, false));
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -363,10 +361,7 @@ public class KalmanFilter {
|
||||||
.add(measurementModel.getMeasurementNoise());
|
.add(measurementModel.getMeasurementNoise());
|
||||||
|
|
||||||
// invert S
|
// invert S
|
||||||
// as the error covariance matrix is a symmetric positive
|
RealMatrix invertedS = MatrixUtils.inverse(s);
|
||||||
// semi-definite matrix, we can use the cholesky decomposition
|
|
||||||
DecompositionSolver solver = new CholeskyDecomposition(s).getSolver();
|
|
||||||
RealMatrix invertedS = solver.getInverse();
|
|
||||||
|
|
||||||
// Inn = z(k) - H * xHat(k)-
|
// Inn = z(k) - H * xHat(k)-
|
||||||
RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation));
|
RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation));
|
||||||
|
|
|
||||||
|
|
@ -14,13 +14,17 @@
|
||||||
|
|
||||||
package org.apache.commons.math3.filter;
|
package org.apache.commons.math3.filter;
|
||||||
|
|
||||||
|
import org.apache.commons.math3.distribution.NormalDistribution;
|
||||||
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
|
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
|
||||||
import org.apache.commons.math3.linear.ArrayRealVector;
|
import org.apache.commons.math3.linear.ArrayRealVector;
|
||||||
import org.apache.commons.math3.linear.MatrixDimensionMismatchException;
|
import org.apache.commons.math3.linear.MatrixDimensionMismatchException;
|
||||||
|
import org.apache.commons.math3.linear.MatrixUtils;
|
||||||
import org.apache.commons.math3.linear.RealMatrix;
|
import org.apache.commons.math3.linear.RealMatrix;
|
||||||
import org.apache.commons.math3.linear.RealVector;
|
import org.apache.commons.math3.linear.RealVector;
|
||||||
import org.apache.commons.math3.random.JDKRandomGenerator;
|
import org.apache.commons.math3.random.JDKRandomGenerator;
|
||||||
import org.apache.commons.math3.random.RandomGenerator;
|
import org.apache.commons.math3.random.RandomGenerator;
|
||||||
|
import org.apache.commons.math3.random.Well19937c;
|
||||||
|
import org.apache.commons.math3.util.FastMath;
|
||||||
import org.apache.commons.math3.util.Precision;
|
import org.apache.commons.math3.util.Precision;
|
||||||
import org.junit.Assert;
|
import org.junit.Assert;
|
||||||
import org.junit.Test;
|
import org.junit.Test;
|
||||||
|
|
@ -212,8 +216,6 @@ public class KalmanFilterTest {
|
||||||
RealVector tmpPNoise = new ArrayRealVector(
|
RealVector tmpPNoise = new ArrayRealVector(
|
||||||
new double[] { Math.pow(dt, 2d) / 2d, dt });
|
new double[] { Math.pow(dt, 2d) / 2d, dt });
|
||||||
|
|
||||||
RealVector mNoise = new ArrayRealVector(1);
|
|
||||||
|
|
||||||
// iterate 60 steps
|
// iterate 60 steps
|
||||||
for (int i = 0; i < 60; i++) {
|
for (int i = 0; i < 60; i++) {
|
||||||
filter.predict(u);
|
filter.predict(u);
|
||||||
|
|
@ -225,10 +227,10 @@ public class KalmanFilterTest {
|
||||||
x = A.operate(x).add(B.operate(u)).add(pNoise);
|
x = A.operate(x).add(B.operate(u)).add(pNoise);
|
||||||
|
|
||||||
// Simulate the measurement
|
// Simulate the measurement
|
||||||
mNoise.setEntry(0, measurementNoise * rand.nextGaussian());
|
double mNoise = measurementNoise * rand.nextGaussian();
|
||||||
|
|
||||||
// z = H * x + m_noise
|
// z = H * x + m_noise
|
||||||
RealVector z = H.operate(x).add(mNoise);
|
RealVector z = H.operate(x).mapAdd(mNoise);
|
||||||
|
|
||||||
filter.correct(z);
|
filter.correct(z);
|
||||||
|
|
||||||
|
|
@ -242,6 +244,182 @@ public class KalmanFilterTest {
|
||||||
0.1d, 1e-6) < 0);
|
0.1d, 1e-6) < 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Represents an idealized Cannonball only taking into account gravity.
|
||||||
|
*/
|
||||||
|
public static class Cannonball {
|
||||||
|
|
||||||
|
private final double[] gravity = { 0, -9.81 };
|
||||||
|
|
||||||
|
private final double[] velocity;
|
||||||
|
private final double[] location;
|
||||||
|
|
||||||
|
private double timeslice;
|
||||||
|
|
||||||
|
public Cannonball(double timeslice, double angle, double initialVelocity) {
|
||||||
|
this.timeslice = timeslice;
|
||||||
|
|
||||||
|
final double angleInRadians = FastMath.toRadians(angle);
|
||||||
|
this.velocity = new double[] {
|
||||||
|
initialVelocity * FastMath.cos(angleInRadians),
|
||||||
|
initialVelocity * FastMath.sin(angleInRadians)
|
||||||
|
};
|
||||||
|
|
||||||
|
this.location = new double[] { 0, 0 };
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getX() {
|
||||||
|
return location[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getY() {
|
||||||
|
return location[1];
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getXVelocity() {
|
||||||
|
return velocity[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getYVelocity() {
|
||||||
|
return velocity[1];
|
||||||
|
}
|
||||||
|
|
||||||
|
public void step() {
|
||||||
|
// break gravitational force into a smaller time slice.
|
||||||
|
double[] slicedGravity = gravity.clone();
|
||||||
|
for ( int i = 0; i < slicedGravity.length; i++ ) {
|
||||||
|
slicedGravity[i] *= timeslice;
|
||||||
|
}
|
||||||
|
|
||||||
|
// apply the acceleration to velocity.
|
||||||
|
double[] slicedVelocity = velocity.clone();
|
||||||
|
for ( int i = 0; i < velocity.length; i++ ) {
|
||||||
|
velocity[i] += slicedGravity[i];
|
||||||
|
slicedVelocity[i] = velocity[i] * timeslice;
|
||||||
|
location[i] += slicedVelocity[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
// cannonballs shouldn't go into the ground.
|
||||||
|
if ( location[1] < 0 ) {
|
||||||
|
location[1] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
@Test
|
||||||
|
public void testCannonball() {
|
||||||
|
// simulates the flight of a cannonball (only taking gravity and initial thrust into account)
|
||||||
|
|
||||||
|
// number of iterations
|
||||||
|
final int iterations = 144;
|
||||||
|
// discrete time interval
|
||||||
|
final double dt = 0.1d;
|
||||||
|
// position measurement noise (meter)
|
||||||
|
final double measurementNoise = 30d;
|
||||||
|
// the initial velocity of the cannonball
|
||||||
|
final double initialVelocity = 100;
|
||||||
|
// shooting angle
|
||||||
|
final double angle = 45;
|
||||||
|
|
||||||
|
final Cannonball cannonball = new Cannonball(dt, angle, initialVelocity);
|
||||||
|
|
||||||
|
final double speedX = cannonball.getXVelocity();
|
||||||
|
final double speedY = cannonball.getYVelocity();
|
||||||
|
|
||||||
|
// A = [ 1, dt, 0, 0 ] => x(n+1) = x(n) + vx(n)
|
||||||
|
// [ 0, 1, 0, 0 ] => vx(n+1) = vx(n)
|
||||||
|
// [ 0, 0, 1, dt ] => y(n+1) = y(n) + vy(n)
|
||||||
|
// [ 0, 0, 0, 1 ] => vy(n+1) = vy(n)
|
||||||
|
final RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
|
||||||
|
{ 1, dt, 0, 0 },
|
||||||
|
{ 0, 1, 0, 0 },
|
||||||
|
{ 0, 0, 1, dt },
|
||||||
|
{ 0, 0, 0, 1 }
|
||||||
|
});
|
||||||
|
|
||||||
|
// The control vector, which adds acceleration to the kinematic equations.
|
||||||
|
// 0 => x(n+1) = x(n+1)
|
||||||
|
// 0 => vx(n+1) = vx(n+1)
|
||||||
|
// -9.81*dt^2 => y(n+1) = y(n+1) - 1/2 * 9.81 * dt^2
|
||||||
|
// -9.81*dt => vy(n+1) = vy(n+1) - 9.81 * dt
|
||||||
|
final RealVector controlVector =
|
||||||
|
MatrixUtils.createRealVector(new double[] { 0, 0, 0.5 * -9.81 * dt * dt, -9.81 * dt } );
|
||||||
|
|
||||||
|
// The control matrix B only expects y and vy, see control vector
|
||||||
|
final RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
|
||||||
|
{ 0, 0, 0, 0 },
|
||||||
|
{ 0, 0, 0, 0 },
|
||||||
|
{ 0, 0, 1, 0 },
|
||||||
|
{ 0, 0, 0, 1 }
|
||||||
|
});
|
||||||
|
|
||||||
|
// We only observe the x/y position of the cannonball
|
||||||
|
final RealMatrix H = MatrixUtils.createRealMatrix(new double[][] {
|
||||||
|
{ 1, 0, 0, 0 },
|
||||||
|
{ 0, 0, 0, 0 },
|
||||||
|
{ 0, 0, 1, 0 },
|
||||||
|
{ 0, 0, 0, 0 }
|
||||||
|
});
|
||||||
|
|
||||||
|
// our guess of the initial state.
|
||||||
|
final RealVector initialState = MatrixUtils.createRealVector(new double[] { 0, speedX, 0, speedY } );
|
||||||
|
|
||||||
|
// the initial error covariance matrix, the variance = noise^2
|
||||||
|
final double var = measurementNoise * measurementNoise;
|
||||||
|
final RealMatrix initialErrorCovariance = MatrixUtils.createRealMatrix(new double[][] {
|
||||||
|
{ var, 0, 0, 0 },
|
||||||
|
{ 0, 1e-3, 0, 0 },
|
||||||
|
{ 0, 0, var, 0 },
|
||||||
|
{ 0, 0, 0, 1e-3 }
|
||||||
|
});
|
||||||
|
|
||||||
|
// we assume no process noise -> zero matrix
|
||||||
|
final RealMatrix Q = MatrixUtils.createRealMatrix(4, 4);
|
||||||
|
|
||||||
|
// the measurement covariance matrix
|
||||||
|
final RealMatrix R = MatrixUtils.createRealMatrix(new double[][] {
|
||||||
|
{ var, 0, 0, 0 },
|
||||||
|
{ 0, 1e-3, 0, 0 },
|
||||||
|
{ 0, 0, var, 0 },
|
||||||
|
{ 0, 0, 0, 1e-3 }
|
||||||
|
});
|
||||||
|
|
||||||
|
final ProcessModel pm = new DefaultProcessModel(A, B, Q, initialState, initialErrorCovariance);
|
||||||
|
final MeasurementModel mm = new DefaultMeasurementModel(H, R);
|
||||||
|
final KalmanFilter filter = new KalmanFilter(pm, mm);
|
||||||
|
|
||||||
|
final RandomGenerator rng = new Well19937c(1000);
|
||||||
|
final NormalDistribution dist = new NormalDistribution(rng, 0, measurementNoise);
|
||||||
|
|
||||||
|
for (int i = 0; i < iterations; i++) {
|
||||||
|
// get the "real" cannonball position
|
||||||
|
double x = cannonball.getX();
|
||||||
|
double y = cannonball.getY();
|
||||||
|
|
||||||
|
// apply measurement noise to current cannonball position
|
||||||
|
double nx = x + dist.sample();
|
||||||
|
double ny = y + dist.sample();
|
||||||
|
|
||||||
|
cannonball.step();
|
||||||
|
|
||||||
|
filter.predict(controlVector);
|
||||||
|
// correct the filter with our measurements
|
||||||
|
filter.correct(new double[] { nx, 0, ny, 0 } );
|
||||||
|
|
||||||
|
// state estimate shouldn't be larger than the measurement noise
|
||||||
|
double diff = FastMath.abs(cannonball.getY() - filter.getStateEstimation()[2]);
|
||||||
|
Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// error covariance of the x/y-position should be already very low (< 3m std dev = 9 variance)
|
||||||
|
|
||||||
|
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[0][0],
|
||||||
|
9, 1e-6) < 0);
|
||||||
|
|
||||||
|
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[2][2],
|
||||||
|
9, 1e-6) < 0);
|
||||||
|
}
|
||||||
|
|
||||||
private void assertVectorEquals(double[] expected, double[] result) {
|
private void assertVectorEquals(double[] expected, double[] result) {
|
||||||
Assert.assertEquals("Wrong number of rows.", expected.length,
|
Assert.assertEquals("Wrong number of rows.", expected.length,
|
||||||
result.length);
|
result.length);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue