diff --git a/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java b/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java new file mode 100644 index 000000000..1ffeb33f1 --- /dev/null +++ b/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java @@ -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 realX = new ArrayList(); + final List realY = new ArrayList(); + final List measuredX = new ArrayList(); + final List measuredY = new ArrayList(); + final List kalmanX = new ArrayList(); + final List kalmanY = new ArrayList(); + + 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()); + } +}