You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@commons.apache.org by tn...@apache.org on 2013/11/07 16:15:18 UTC
svn commit: r1539676 - in /commons/proper/math/trunk/src: changes/changes.xml
main/java/org/apache/commons/math3/filter/KalmanFilter.java
test/java/org/apache/commons/math3/filter/KalmanFilterTest.java
Author: tn
Date: Thu Nov 7 15:15:18 2013
New Revision: 1539676
URL: http://svn.apache.org/r1539676
Log:
[MATH-1062] Use MatrixUtils.inverse to invert a matrix in the KalmanFilter, added new unit test.
Modified:
commons/proper/math/trunk/src/changes/changes.xml
commons/proper/math/trunk/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java
commons/proper/math/trunk/src/test/java/org/apache/commons/math3/filter/KalmanFilterTest.java (contents, props changed)
Modified: commons/proper/math/trunk/src/changes/changes.xml
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/changes/changes.xml?rev=1539676&r1=1539675&r2=1539676&view=diff
==============================================================================
--- commons/proper/math/trunk/src/changes/changes.xml (original)
+++ commons/proper/math/trunk/src/changes/changes.xml Thu Nov 7 15:15:18 2013
@@ -51,6 +51,10 @@ If the output is not quite correct, chec
</properties>
<body>
<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">
Precision improvements (for small values of the argument) in "Beta" function
and in "LogNormalDistribution" and "WeibullDistribution".
Modified: commons/proper/math/trunk/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java?rev=1539676&r1=1539675&r2=1539676&view=diff
==============================================================================
--- commons/proper/math/trunk/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java (original)
+++ commons/proper/math/trunk/src/main/java/org/apache/commons/math3/filter/KalmanFilter.java Thu Nov 7 15:15:18 2013
@@ -20,8 +20,6 @@ import org.apache.commons.math3.exceptio
import org.apache.commons.math3.exception.NullArgumentException;
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
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.MatrixUtils;
import org.apache.commons.math3.linear.NonSquareMatrixException;
@@ -283,7 +281,7 @@ public class KalmanFilter {
* if the dimension of the control vector does not fit
*/
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)
throws NullArgumentException, DimensionMismatchException, SingularMatrixException {
- correct(new ArrayRealVector(z));
+ correct(new ArrayRealVector(z, false));
}
/**
@@ -363,10 +361,7 @@ public class KalmanFilter {
.add(measurementModel.getMeasurementNoise());
// invert S
- // as the error covariance matrix is a symmetric positive
- // semi-definite matrix, we can use the cholesky decomposition
- DecompositionSolver solver = new CholeskyDecomposition(s).getSolver();
- RealMatrix invertedS = solver.getInverse();
+ RealMatrix invertedS = MatrixUtils.inverse(s);
// Inn = z(k) - H * xHat(k)-
RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation));
Modified: commons/proper/math/trunk/src/test/java/org/apache/commons/math3/filter/KalmanFilterTest.java
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/test/java/org/apache/commons/math3/filter/KalmanFilterTest.java?rev=1539676&r1=1539675&r2=1539676&view=diff
==============================================================================
--- commons/proper/math/trunk/src/test/java/org/apache/commons/math3/filter/KalmanFilterTest.java (original)
+++ commons/proper/math/trunk/src/test/java/org/apache/commons/math3/filter/KalmanFilterTest.java Thu Nov 7 15:15:18 2013
@@ -14,13 +14,17 @@
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.ArrayRealVector;
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.RealVector;
import org.apache.commons.math3.random.JDKRandomGenerator;
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.junit.Assert;
import org.junit.Test;
@@ -212,8 +216,6 @@ public class KalmanFilterTest {
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);
@@ -225,10 +227,10 @@ public class KalmanFilterTest {
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
- mNoise.setEntry(0, measurementNoise * rand.nextGaussian());
+ double mNoise = measurementNoise * rand.nextGaussian();
// z = H * x + m_noise
- RealVector z = H.operate(x).add(mNoise);
+ RealVector z = H.operate(x).mapAdd(mNoise);
filter.correct(z);
@@ -241,7 +243,183 @@ public class KalmanFilterTest {
Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
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) {
Assert.assertEquals("Wrong number of rows.", expected.length,
result.length);
Propchange: commons/proper/math/trunk/src/test/java/org/apache/commons/math3/filter/KalmanFilterTest.java
------------------------------------------------------------------------------
svn:keywords = Id Revision