commons-dev mailing list archives

Site index · List index
Message view « Date » · « Thread »
Top « Date » · « Thread »
From Phil Steitz <phil.ste...@gmail.com>
Subject Re: svn commit: r1539723 - /commons/proper/math/trunk/src/userguide/java/org/apache/commons/math3/userguide/filter/CannonballExample.java
Date Thu, 07 Nov 2013 17:28:50 GMT
Thanks so much for starting to build these out, Thomas!

Patches welcome for more sample code :)

Phil
On 11/7/13 9:16 AM, tn@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-unsubscribe@commons.apache.org
For additional commands, e-mail: dev-help@commons.apache.org


Mime
View raw message