KalmanSmoother.java

  1. /*
  2.  * Licensed to the Hipparchus project under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *      https://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.hipparchus.filtering.kalman;

  19. import org.hipparchus.exception.MathIllegalStateException;
  20. import org.hipparchus.filtering.LocalizedFilterFormats;
  21. import org.hipparchus.linear.MatrixDecomposer;
  22. import org.hipparchus.linear.RealMatrix;
  23. import org.hipparchus.linear.RealVector;

  25. import java.util.ArrayList;
  26. import java.util.LinkedList;
  27. import java.util.List;

  29. /**
  30.  * Kalman smoother for linear, extended or unscented filters.
  31.  * <p>
  32.  * This implementation is attached to a filter using the observer mechanism.  Once all measurements have been
  33.  * processed by the filter, the smoothing method can be called.
  34.  * </p>
  35.  * <p>
  36.  * For example
  37.  * </p>
  38.  * <pre>{@code
  39.  *     // Kalman filter
  40.  *     final KalmanFilter<SimpleMeasurement> filter = new LinearKalmanFilter<>(decomposer, process, initialState);
  41.  *
  42.  *     // Smoother observer
  43.  *     final KalmanSmoother smoother = new KalmanSmoother(decomposer);
  44.  *     filter.setObserver(smoother);
  45.  *
  46.  *     // Process measurements with filter (forwards pass)
  47.  *     measurements.forEach(filter::estimationStep);
  48.  *
  49.  *     // Smooth backwards
  50.  *     List<ProcessEstimate> smoothedStates = smoother.backwardsSmooth();
  51.  * }</pre>
  52.  *
  53.  * @see "Särkkä, S. Bayesian Filtering and Smoothing. Cambridge 2013"
  54.  */
  55. public class KalmanSmoother implements KalmanObserver {

  57.     /** Decomposer to use for gain calculation. */
  58.     private final MatrixDecomposer decomposer;

  60.     /** Storage for smoother gain matrices. */
  61.     private final List<SmootherData> smootherData;

  63.     /** Simple constructor.
  64.      * @param decomposer decomposer to use for the smoother gain calculations
  65.      */
  66.     public KalmanSmoother(final MatrixDecomposer decomposer) {
  67.         this.decomposer = decomposer;
  68.         this.smootherData = new ArrayList<>();
  69.     }

  71.     @Override
  72.     public void init(KalmanEstimate estimate) {
  73.         // Add initial state to smoother data
  74.         smootherData.add(new SmootherData(
  75.                 estimate.getCorrected().getTime(),
  76.                 null,
  77.                 null,
  78.                 estimate.getCorrected().getState(),
  79.                 estimate.getCorrected().getCovariance(),
  80.                 null
  81.         ));

  83.     }

  85.     @Override
  86.     public void updatePerformed(KalmanEstimate estimate) {
  87.         // Smoother gain
  88.         // We want G = D * P^(-1)
  89.         // Calculate with G = (P^(-1) * D^T)^T
  90.         final RealMatrix smootherGain = decomposer
  91.                 .decompose(estimate.getPredicted().getCovariance())
  92.                 .solve(estimate.getStateCrossCovariance().transpose())
  93.                 .transpose();
  94.         smootherData.add(new SmootherData(
  95.                 estimate.getCorrected().getTime(),
  96.                 estimate.getPredicted().getState(),
  97.                 estimate.getPredicted().getCovariance(),
  98.                 estimate.getCorrected().getState(),
  99.                 estimate.getCorrected().getCovariance(),
  100.                 smootherGain
  101.         ));
  102.     }

  104.     /** Backwards smooth.
  105.      * This is a backward pass over the filtered data, recursively calculating smoothed states, using the
  106.      * Rauch-Tung-Striebel (RTS) formulation.
  107.      * Note that the list result is a `LinkedList`, not an `ArrayList`.
  108.      * @return list of smoothed states
  109.      */
  110.     public List<ProcessEstimate> backwardsSmooth() {
  111.         // Check for at least one measurement
  112.         if (smootherData.size() < 2) {
  113.             throw new MathIllegalStateException(LocalizedFilterFormats.PROCESS_AT_LEAST_ONE_MEASUREMENT);
  114.         }

  116.         // Initialise output
  117.         final LinkedList<ProcessEstimate> smootherResults = new LinkedList<>();

  119.         // Last smoothed state is the same as the filtered state
  120.         final SmootherData lastUpdate = smootherData.get(smootherData.size() - 1);
  121.         ProcessEstimate smoothedState = new ProcessEstimate(lastUpdate.getTime(),
  122.                 lastUpdate.getCorrectedState(), lastUpdate.getCorrectedCovariance());
  123.         smootherResults.addFirst(smoothedState);

  125.         // Backwards recursion on the smoothed state
  126.         for (int i = smootherData.size() - 2; i >= 0; --i) {

  128.             // These are from equation 8.6 in Sarkka, "Bayesian Filtering and Smoothing", Cambridge, 2013.
  129.             final RealMatrix smootherGain = smootherData.get(i + 1).getSmootherGain();

  131.             final RealVector smoothedMean = smootherData.get(i).getCorrectedState()
  132.                     .add(smootherGain.operate(smoothedState.getState()
  133.                             .subtract(smootherData.get(i + 1).getPredictedState())));

  135.             final RealMatrix smoothedCovariance = smootherData.get(i).getCorrectedCovariance()
  136.                     .add(smootherGain.multiply(smoothedState.getCovariance()
  137.                                     .subtract(smootherData.get(i + 1).getPredictedCovariance()))
  138.                             .multiplyTransposed(smootherGain));

  140.             // Populate smoothed state
  141.             smoothedState = new ProcessEstimate(smootherData.get(i).getTime(), smoothedMean, smoothedCovariance);
  142.             smootherResults.addFirst(smoothedState);
  143.         }

  145.         return smootherResults;
  146.     }

  148.     /** Container for smoother data. */
  149.     private static class SmootherData {
  150.         /** Process time (typically the time or index of a measurement). */
  151.         private final double time;

  153.         /** Predicted state vector. */
  154.         private final RealVector predictedState;

  156.         /** Predicted covariance. */
  157.         private final RealMatrix predictedCovariance;

  159.         /** Corrected state vector. */
  160.         private final RealVector correctedState;

  162.         /** Corrected covariance. */
  163.         private final RealMatrix correctedCovariance;

  165.         /** Smoother gain. */
  166.         private final RealMatrix smootherGain;

  168.         SmootherData(final double time,
  169.                      final RealVector predictedState,
  170.                      final RealMatrix predictedCovariance,
  171.                      final RealVector correctedState,
  172.                      final RealMatrix correctedCovariance,
  173.                      final RealMatrix smootherGain) {
  174.             this.time = time;
  175.             this.predictedState = predictedState;
  176.             this.predictedCovariance = predictedCovariance;
  177.             this.correctedState = correctedState;
  178.             this.correctedCovariance = correctedCovariance;
  179.             this.smootherGain = smootherGain;
  180.         }

  182.         /** Get the process time.
  183.          * @return process time (typically the time or index of a measurement)
  184.          */
  185.         public double getTime() {
  186.             return time;
  187.         }

  189.         /**
  190.          * Get predicted state
  191.          * @return predicted state
  192.          */
  193.         public RealVector getPredictedState() {
  194.             return predictedState;
  195.         }

  197.         /**
  198.          * Get predicted covariance
  199.          * @return predicted covariance
  200.          */
  201.         public RealMatrix getPredictedCovariance() {
  202.             return predictedCovariance;
  203.         }

  205.         /**
  206.          * Get corrected state
  207.          * @return corrected state
  208.          */
  209.         public RealVector getCorrectedState() {
  210.             return correctedState;
  211.         }

  213.         /**
  214.          * Get corrected covariance
  215.          * @return corrected covariance
  216.          */
  217.         public RealMatrix getCorrectedCovariance() {
  218.             return correctedCovariance;
  219.         }

  221.         /**
  222.          * Get smoother gain (for previous time-step)
  223.          * @return smoother gain
  224.          */
  225.         public RealMatrix getSmootherGain() {
  226.             return smootherGain;
  227.         }
  228.     }

  230. }
Created with JaCoCo 0.8.12.202403310830