Thanks so much for starting to build these out, Thomas! Patches welcome for more sample code :)
Phil On 11/7/13 9:16 AM, t...@apache.org wrote: > Author: tn > Date: Thu Nov 7 17:16:04 2013 > New Revision: 1539723 > > URL: http://svn.apache.org/r1539723 > Log: > Add cannonball kalman filter example for user guide > > Added: > > commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java > (with props) > > Added: > commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java > URL: > http://svn.apache.org/viewvc/commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java?rev=1539723&view=auto > ============================================================================== > --- > commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java > (added) > +++ > commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java > Thu Nov 7 17:16:04 2013 > @@ -0,0 +1,330 @@ > +/* > + * 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. > + */ > + > +package org.apache.commons.math3.userguide.filter; > + > +import java.awt.Color; > +import java.awt.Component; > +import java.awt.Font; > +import java.util.ArrayList; > +import java.util.List; > + > +import javax.swing.BorderFactory; > +import javax.swing.BoxLayout; > +import javax.swing.JComponent; > +import javax.swing.JPanel; > + > +import org.apache.commons.math3.filter.DefaultMeasurementModel; > +import org.apache.commons.math3.filter.DefaultProcessModel; > +import org.apache.commons.math3.filter.KalmanFilter; > +import org.apache.commons.math3.filter.MeasurementModel; > +import org.apache.commons.math3.filter.ProcessModel; > +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.RandomGenerator; > +import org.apache.commons.math3.random.Well19937c; > +import org.apache.commons.math3.userguide.ExampleUtils; > +import org.apache.commons.math3.userguide.ExampleUtils.ExampleFrame; > +import org.apache.commons.math3.util.FastMath; > + > +import com.xeiam.xchart.Chart; > +import com.xeiam.xchart.ChartBuilder; > +import com.xeiam.xchart.Series; > +import com.xeiam.xchart.SeriesLineStyle; > +import com.xeiam.xchart.SeriesMarker; > +import com.xeiam.xchart.XChartPanel; > +import com.xeiam.xchart.StyleManager.ChartType; > +import com.xeiam.xchart.StyleManager.LegendPosition; > + > +public class CannonballExample { > + > + public static class Cannonball { > + > + private final double[] gravity = { 0, -9.81 }; > + private final double[] velocity; > + private final double[] location; > + > + private final double timeslice; > + private final double measurementNoise; > + > + private final RandomGenerator rng; > + > + public Cannonball(double timeslice, double angle, double > initialVelocity, double measurementNoise, int seed) { > + 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 }; > + > + this.measurementNoise = measurementNoise; > + this.rng = new Well19937c(seed); > + } > + > + public double getX() { > + return location[0]; > + } > + > + public double getY() { > + return location[1]; > + } > + > + public double getMeasuredX() { > + return location[0] + rng.nextGaussian() * measurementNoise; > + } > + > + public double getMeasuredY() { > + return location[1] + rng.nextGaussian() * measurementNoise; > + } > + > + 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; > + } > + } > + } > + > + public static void cannonballTest(Chart chart) { > + > + // Let's go over the physics behind the cannon shot, just to make > sure it's > + // correct: > + // sin(45)*100 = 70.710 and cos(45)*100 = 70.710 > + // vf = vo + at > + // 0 = 70.710 + (-9.81)t > + // t = 70.710/9.81 = 7.208 seconds for half > + // 14.416 seconds for full journey > + // distance = 70.710 m/s * 14.416 sec = 1019.36796 m > + > + // time interval for each iteration > + final double dt = 0.1; > + // the number of iterations to run > + final int iterations = 144; > + // measurement noise (m) > + final double measurementNoise = 30; > + // initial velocity of the cannonball > + final double initialVelocity = 100; > + // shooting angle > + final double angle = 45; > + > + // the cannonball itself > + final Cannonball cannonball = new Cannonball(dt, angle, > initialVelocity, measurementNoise, 1000); > + > + // 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 } > + }); > + > + // After state transition and control, here are the equations: > + // > + // x(n+1) = x(n) + vx(n) > + // vx(n+1) = vx(n) > + // y(n+1) = y(n) + vy(n) - 0.5*9.81*dt^2 > + // vy(n+1) = vy(n) + -9.81*dt > + // > + // Which, if you recall, are the equations of motion for a parabola. > + > + // 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 } > + }); > + > + // This is our guess of the initial state. I intentionally set the > Y value > + // wrong to illustrate how fast the Kalman filter will pick up on > that. > + final double speedX = cannonball.getXVelocity(); > + final double speedY = cannonball.getYVelocity(); > + final RealVector initialState = MatrixUtils.createRealVector(new > double[] { 0, speedX, 100, 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 List<Number> realX = new ArrayList<Number>(); > + final List<Number> realY = new ArrayList<Number>(); > + final List<Number> measuredX = new ArrayList<Number>(); > + final List<Number> measuredY = new ArrayList<Number>(); > + final List<Number> kalmanX = new ArrayList<Number>(); > + final List<Number> kalmanY = new ArrayList<Number>(); > + > + for (int i = 0; i < iterations; i++) { > + > + // get real location > + realX.add(cannonball.getX()); > + realY.add(cannonball.getY()); > + > + // get measured location > + final double mx = cannonball.getMeasuredX(); > + final double my = cannonball.getMeasuredY(); > + > + measuredX.add(mx); > + measuredY.add(my); > + > + // iterate the cannon simulation to the next timeslice. > + cannonball.step(); > + > + final double[] state = filter.getStateEstimation(); > + kalmanX.add(state[0]); > + kalmanY.add(state[2]); > + > + // update the kalman filter with the measurements > + filter.predict(controlVector); > + filter.correct(new double[] { mx, 0, my, 0 } ); > + } > + > + chart.setXAxisTitle("Distance (m)"); > + chart.setYAxisTitle("Height (m)"); > + > + Series dataset = chart.addSeries("true", realX, realY); > + dataset.setMarker(SeriesMarker.NONE); > + > + dataset = chart.addSeries("measured", measuredX, measuredY); > + dataset.setLineStyle(SeriesLineStyle.DOT_DOT); > + dataset.setMarker(SeriesMarker.NONE); > + > + dataset = chart.addSeries("kalman", kalmanX, kalmanY); > + dataset.setLineColor(Color.red); > + dataset.setLineStyle(SeriesLineStyle.DASH_DASH); > + dataset.setMarker(SeriesMarker.NONE); > + } > + > + public static Chart createChart(String title, LegendPosition position) { > + Chart chart = new ChartBuilder().width(600).height(400).build(); > + > + // Customize Chart > + chart.setChartTitle(title); > + chart.getStyleManager().setChartTitleVisible(true); > + chart.getStyleManager().setChartTitleFont(new Font("Arial", > Font.PLAIN, 10)); > + chart.getStyleManager().setLegendPosition(position); > + chart.getStyleManager().setLegendVisible(true); > + chart.getStyleManager().setLegendFont(new Font("Arial", Font.PLAIN, > 10)); > + chart.getStyleManager().setLegendPadding(6); > + chart.getStyleManager().setLegendSeriesLineLength(10); > + chart.getStyleManager().setAxisTickLabelsFont(new Font("Arial", > Font.PLAIN, 9)); > + > + chart.getStyleManager().setChartBackgroundColor(Color.white); > + chart.getStyleManager().setChartPadding(4); > + > + chart.getStyleManager().setChartType(ChartType.Line); > + return chart; > + } > + > + public static JComponent createComponent() { > + JComponent container = new JPanel(); > + container.setLayout(new BoxLayout(container, BoxLayout.PAGE_AXIS)); > + > + Chart chart = createChart("Cannonball", LegendPosition.InsideNE); > + cannonballTest(chart); > + container.add(new XChartPanel(chart)); > + > + container.setBorder(BorderFactory.createLineBorder(Color.black, 1)); > + return container; > + } > + > + @SuppressWarnings("serial") > + public static class Display extends ExampleFrame { > + > + private JComponent container; > + > + public Display() { > + setTitle("Commons Math: Kalman Filter - Cannonball"); > + setSize(800, 600); > + > + container = new JPanel(); > + JComponent comp = createComponent(); > + container.add(comp); > + > + add(container); > + } > + > + @Override > + public Component getMainPanel() { > + return container; > + } > + } > + > + public static void main(String[] args) { > + ExampleUtils.showExampleFrame(new Display()); > + } > +} > > Propchange: > commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java > ------------------------------------------------------------------------------ > svn:eol-style = native > > Propchange: > commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java > ------------------------------------------------------------------------------ > svn:keywords = Id Revision HeadURL > > Propchange: > commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java > ------------------------------------------------------------------------------ > svn:mime-type = text/plain > > > --------------------------------------------------------------------- To unsubscribe, e-mail: dev-unsubscr...@commons.apache.org For additional commands, e-mail: dev-h...@commons.apache.org
