| <?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"?> |
| <document url="filter.html"> |
| <properties> |
| <title>The Commons Math User Guide - Filters</title> |
| </properties> |
| <body> |
| <section name="17 Filters"> |
| <subsection name="17.1 Overview" href="overview"> |
| <p> |
| The filter package currently provides only an implementation of a Kalman filter. |
| </p> |
| </subsection> |
| <subsection name="17.2 Kalman Filter" href="kalman"> |
| <p> |
| <a href="../apidocs/org/apache/commons/math4/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/math4/filter/ProcessModel.html"> |
| ProcessModel</a> and a <a href="../apidocs/org/apache/commons/math4/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 < 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 < 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> |