commons-commits mailing list archives

Site index · List index
Message view « Date » · « Thread »
Top « Date » · « Thread »
From l..@apache.org
Subject svn commit: r1208043 - in /commons/proper/math/trunk/src: site/site.xml site/xdoc/userguide/filter.xml site/xdoc/userguide/index.xml test/java/org/apache/commons/math/filter/KalmanFilterTest.java
Date Tue, 29 Nov 2011 19:36:53 GMT
Author: luc
Date: Tue Nov 29 19:36:52 2011
New Revision: 1208043

URL: http://svn.apache.org/viewvc?rev=1208043&view=rev
Log:
Updated tests and documentation for Kalman filter.

Patch provided by Thomas Neidhart.

Added:
    commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml   (with props)
Modified:
    commons/proper/math/trunk/src/site/site.xml
    commons/proper/math/trunk/src/site/xdoc/userguide/index.xml
    commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java

Modified: commons/proper/math/trunk/src/site/site.xml
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/site/site.xml?rev=1208043&r1=1208042&r2=1208043&view=diff
==============================================================================
--- commons/proper/math/trunk/src/site/site.xml (original)
+++ commons/proper/math/trunk/src/site/site.xml Tue Nov 29 19:36:52 2011
@@ -65,6 +65,7 @@
       <item name="Optimization"            href="/userguide/optimization.html"/>
       <item name="Ordinary Differential Equations" href="/userguide/ode.html"/>
       <item name="Genetic Algorithms"      href="/userguide/genetics.html"/>
+      <item name="Filters"                 href="/userguide/filter.html"/>
     </menu>
 
   </body>

Added: commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml?rev=1208043&view=auto
==============================================================================
--- commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml (added)
+++ commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml Tue Nov 29 19:36:52 2011
@@ -0,0 +1,227 @@
+<?xml version="1.0"?>
+
+<!--
+    Licensed to the Apache Software Foundation (ASF) under one or more
+    contributor license agreements.  See the NOTICE file distributed with
+    this work for additional information regarding copyright ownership.
+    The ASF licenses this file to You under the Apache License, Version 2.0
+    (the "License"); you may not use this file except in compliance with
+    the License.  You may obtain a copy of the License at
+   
+         http://www.apache.org/licenses/LICENSE-2.0
+   
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+  -->
+  
+<?xml-stylesheet type="text/xsl" href="./xdoc.xsl"?>
+<!-- $Revision: 937893 $ $Date: 2010-04-25 23:42:47 +0200 (Sun, 25 Apr 2010) $ -->
+<document url="filter.html">
+  <properties>
+    <title>The Commons Math User Guide - Filters</title>
+  </properties>
+  <body>
+    <section name="15 Filters">
+      <subsection name="15.1 Overview" href="overview">
+        <p>
+          The filter package currently provides only an implementation of a Kalman filter.
+        </p>
+      </subsection>
+      <subsection name="15.2 Kalman Filter" href="kalman">
+        <p>
+          <a href="../apidocs/org/apache/commons/math/filter/KalmanFilter.html">
+          KalmanFilter</a> provides a discrete-time filter to estimate
+          a stochastic linear process.</p>
+          
+        <p>A Kalman filter is initialized with a <a href="../apidocs/org/apache/commons/math/filter/ProcessModel.html">
+          ProcessModel</a> and a <a href="../apidocs/org/apache/commons/math/filter/MeasurementModel.html">
+          MeasurementModel</a>, which contain the corresponding transformation and
noise covariance matrices. 
+          The parameter names used in the respective models correspond to the following names
commonly used 
+          in the mathematical literature:
+		<ul>
+ 			<li>A - state transition matrix</li>
+ 			<li>B - control input matrix</li>
+ 			<li>H - measurement matrix</li>
+ 			<li>Q - process noise covariance matrix</li>
+ 			<li>R - measurement noise covariance matrix</li>
+ 			<li>P - error covariance matrix</li>
+ 		</ul>
+ 		</p>
+        <p>
+        <dl>
+      	  <dt>Initialization</dt>
+          <dd> The following code will create a Kalman filter using the provided 
+          DefaultMeasurementModel and DefaultProcessModel classes. To support dynamically
changing
+          process and measurement noises, simply implement your own models.
+          	<source>
+// A = [ 1 ]
+RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
+// no control input
+RealMatrix B = null;
+// H = [ 1 ]
+RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
+// Q = [ 0 ]
+RealMatrix Q = new Array2DRowRealMatrix(new double[] { 0 });
+// R = [ 0 ]
+RealMatrix R = new Array2DRowRealMatrix(new double[] { 0 });
+
+ProcessModel pm
+   = new DefaultProcessModel(A, B, Q, new ArrayRealVector(new double[] { 0 }), null);
+MeasurementModel mm = new DefaultMeasurementModel(H, R);
+KalmanFilter filter = new KalmanFilter(pm, mm);
+ 			</source>
+		  </dd>
+		  <dt>Iteration</dt>
+          <dd>The following code illustrates how to perform the predict/correct cycle:
 
+		  <source>
+for (;;) {
+   // predict the state estimate one time-step ahead
+   // optionally provide some control input
+   filter.predict();
+
+   // obtain measurement vector z
+   RealVector z = getMeasurement();
+
+   // correct the state estimate with the latest measurement
+   filter.correct(z);
+   
+   double[] stateEstimate = filter.getStateEstimation();
+   // do something with it
+}
+		  </source>
+		  </dd>
+		  <dt>Constant Voltage Example</dt>
+          <dd>The following example creates a Kalman filter for a static process: a
system with a 
+          constant voltage as internal state. We observe this process with an artificially

+          imposed measurement noise of 0.1V and assume an internal process noise of 1e-5V.
+          <source>
+double constantVoltage = 10d;
+double measurementNoise = 0.1d;
+double processNoise = 1e-5d;
+
+// A = [ 1 ]
+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 &lt; 60; i++) {
+    filter.predict();
+
+    // simulate the process
+    pNoise.setEntry(0, processNoise * rand.nextGaussian());
+
+    // x = A * x + p_noise
+    x = A.operate(x).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);
+
+	double voltage = filter.getStateEstimation()[0];
+}
+          </source>
+          </dd>
+      	  <dt>Increasing Speed Vehicle Example</dt>
+          <dd>The following example creates a Kalman filter for a simple linear process:
a 
+          vehicle driving along a street with a velocity increasing at a constant rate. The
process
+          state is modeled as (position, velocity) and we only observe the position. A measurement
+          noise of 10m is imposed on the simulated measurement.
+          <source>
+// 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);
+
+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 &lt; 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);
+
+	double position = filter.getStateEstimation()[0];
+	double velocity = filter.getStateEstimation()[1];
+}
+          </source>
+          </dd>          
+        </dl>
+        </p>        
+      </subsection>
+    </section>
+  </body>
+</document>

Propchange: commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml
------------------------------------------------------------------------------
    svn:eol-style = native

Propchange: commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml
------------------------------------------------------------------------------
    svn:keywords = "Author Date Id Revision"

Modified: commons/proper/math/trunk/src/site/xdoc/userguide/index.xml
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/site/xdoc/userguide/index.xml?rev=1208043&r1=1208042&r2=1208043&view=diff
==============================================================================
--- commons/proper/math/trunk/src/site/xdoc/userguide/index.xml (original)
+++ commons/proper/math/trunk/src/site/xdoc/userguide/index.xml Tue Nov 29 19:36:52 2011
@@ -142,6 +142,11 @@
                 <li><a href="genetics.html#a14.2_GA_Framework">14.2 GA Framework</a></li>
                 <li><a href="genetics.html#a14.3_Implementation">14.3 Implementation
and Examples</a></li>  
                 </ul></li>                           
+        <li><a href="filter.html">15. Filters</a>   
+                <ul>
+                <li><a href="filter.html#a15.1_Overview">15.1 Overview</a></li>
+                <li><a href="filter.html#a15.2_Kalman_Filter">15.2 Kalman Filter</a></li>
+                </ul></li>                           
         </ul>
       </section>
     

Modified: commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java?rev=1208043&r1=1208042&r2=1208043&view=diff
==============================================================================
--- commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
(original)
+++ commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
Tue Nov 29 19:36:52 2011
@@ -16,6 +16,7 @@ package org.apache.commons.math.filter;
 
 import org.apache.commons.math.linear.Array2DRowRealMatrix;
 import org.apache.commons.math.linear.ArrayRealVector;
+import org.apache.commons.math.linear.MatrixDimensionMismatchException;
 import org.apache.commons.math.linear.RealMatrix;
 import org.apache.commons.math.linear.RealVector;
 import org.apache.commons.math.random.JDKRandomGenerator;
@@ -25,13 +26,64 @@ import org.junit.Assert;
 import org.junit.Test;
 
 /**
- * Test for {@link KalmanFilter}.
+ * Tests for {@link KalmanFilter}.
  *
  * @version $Id$
  */
 public class KalmanFilterTest {
+    
+    @Test(expected=MatrixDimensionMismatchException.class)
+    public void testTransitionMeasurementMatrixMismatch() {
+        
+        // A and H matrix do not match in dimensions
+        
+        // A = [ 1 ]
+        RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
+        // no control input
+        RealMatrix B = null;
+        // H = [ 1 1 ]
+        RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d, 1d });
+        // Q = [ 0 ]
+        RealMatrix Q = new Array2DRowRealMatrix(new double[] { 0 });
+        // R = [ 0 ]
+        RealMatrix R = new Array2DRowRealMatrix(new double[] { 0 });
+
+        ProcessModel pm
+            = new DefaultProcessModel(A, B, Q,
+                                      new ArrayRealVector(new double[] { 0 }), null);
+        MeasurementModel mm = new DefaultMeasurementModel(H, R);
+        new KalmanFilter(pm, mm);
+        Assert.fail("transition and measurement matrix should not be compatible");
+    }
+
+    @Test(expected=MatrixDimensionMismatchException.class)
+    public void testTransitionControlMatrixMismatch() {
+        
+        // A and B matrix do not match in dimensions
+        
+        // A = [ 1 ]
+        RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
+        // B = [ 1 1 ]
+        RealMatrix B = new Array2DRowRealMatrix(new double[] { 1d, 1d });
+        // H = [ 1 ]
+        RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
+        // Q = [ 0 ]
+        RealMatrix Q = new Array2DRowRealMatrix(new double[] { 0 });
+        // R = [ 0 ]
+        RealMatrix R = new Array2DRowRealMatrix(new double[] { 0 });
+
+        ProcessModel pm
+            = new DefaultProcessModel(A, B, Q,
+                                      new ArrayRealVector(new double[] { 0 }), null);
+        MeasurementModel mm = new DefaultMeasurementModel(H, R);
+        new KalmanFilter(pm, mm);
+        Assert.fail("transition and control matrix should not be compatible");
+    }
+    
     @Test
     public void testConstant() {
+        // simulates a simple process with a constant state and no control input
+        
         double constantValue = 10d;
         double measurementNoise = 0.1d;
         double processNoise = 1e-5d;
@@ -86,7 +138,7 @@ public class KalmanFilterTest {
 
             filter.correct(z);
 
-            // state estimate should be larger than measurement noise
+            // state estimate shouldn't be larger than measurement noise
             double diff = Math.abs(constantValue - filter.getStateEstimation()[0]);
             // System.out.println(diff);
             Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
@@ -97,12 +149,105 @@ public class KalmanFilterTest {
                                               0.02d, 1e-6) < 0);
     }
 
+    @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);
+    }
+    
     private void assertVectorEquals(double[] expected, double[] result) {
         Assert.assertEquals("Wrong number of rows.", expected.length,
                             result.length);
         for (int i = 0; i < expected.length; i++) {
             Assert.assertEquals("Wrong value at position [" + i + "]",
-                                expected[i], result[i], 1.0e-15);
+                                expected[i], result[i], 1.0e-6);
         }
     }
 
@@ -114,7 +259,7 @@ public class KalmanFilterTest {
                                 result[i].length);
             for (int j = 0; j < expected[i].length; j++) {
                 Assert.assertEquals("Wrong value at position [" + i + "," + j
-                                    + "]", expected[i][j], result[i][j], 1.0e-15);
+                                    + "]", expected[i][j], result[i][j], 1.0e-6);
             }
         }
     }



Mime
View raw message