src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java - commons-math - Git at Google

 /*
  * 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) {

         // 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 update 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(650).height(450).build();

         // Customize Chart
         chart.setChartTitle(title);
         chart.getStyleManager().setChartTitleVisible(true);
         chart.getStyleManager().setChartTitleFont(new Font("Arial", Font.PLAIN, 12));
         chart.getStyleManager().setLegendPosition(position);
         chart.getStyleManager().setLegendVisible(true);
         chart.getStyleManager().setLegendFont(new Font("Arial", Font.PLAIN, 12));
         chart.getStyleManager().setLegendPadding(6);
         chart.getStyleManager().setLegendSeriesLineLength(10);
         chart.getStyleManager().setAxisTickLabelsFont(new Font("Arial", Font.PLAIN, 10));

         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());
     }
 }
	/*
	* 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) {

	// 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.81dt^2 => y(n+1) = y(n+1) - 1/2 9.81 * dt^2
	// -9.81dt => 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 update 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(650).height(450).build();

	// Customize Chart
	chart.setChartTitle(title);
	chart.getStyleManager().setChartTitleVisible(true);
	chart.getStyleManager().setChartTitleFont(new Font("Arial", Font.PLAIN, 12));
	chart.getStyleManager().setLegendPosition(position);
	chart.getStyleManager().setLegendVisible(true);
	chart.getStyleManager().setLegendFont(new Font("Arial", Font.PLAIN, 12));
	chart.getStyleManager().setLegendPadding(6);
	chart.getStyleManager().setLegendSeriesLineLength(10);
	chart.getStyleManager().setAxisTickLabelsFont(new Font("Arial", Font.PLAIN, 10));

	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());
	}
	}