commons-commits mailing list archives

Site index · List index
Message view « Date » · « Thread »
Top « Date » · « Thread »
From t.@apache.org
Subject 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
Date Thu, 07 Nov 2013 15:15:18 GMT
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



Mime
View raw message