Issue 363 - Kalman smoother (#364)

* Linear and extended Kalman smoothers.

* Unscented Kalman smoother.

* Smoother as Kalman observer.

* Check for processing at least one measurement.

Author: mgrutten

hipparchus-filtering/src/main/java/org/hipparchus/filtering/LocalizedFilterFormats.java

@@ -0,0 +1,71 @@
+/*
+ * 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
+ *
+ *      https://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.
+ */
+
+/*
+ * This is not the original file distributed by the Apache Software Foundation
+ * It has been modified by the Hipparchus project
+ */
+package org.hipparchus.filtering;
+
+import org.hipparchus.exception.Localizable;
+
+import java.util.Locale;
+
+/**
+ * Enumeration for localized messages formats used in exceptions messages.
+ * <p>
+ * The constants in this enumeration represent the available
+ * formats as localized strings. These formats are intended to be
+ * localized using simple properties files, using the constant
+ * name as the key and the property value as the message format.
+ * The source English format is provided in the constants themselves
+ * to serve both as a reminder for developers to understand the parameters
+ * needed by each format, as a basis for translators to create
+ * localized properties files, and as a default format if some
+ * translation is missing.
+ * </p>
+ */
+public enum LocalizedFilterFormats implements Localizable {
+
+    /** PROCESS_AT_LEAST_ONE_MEASUREMENT. */
+    PROCESS_AT_LEAST_ONE_MEASUREMENT("at least one measurement must be processed before smoothing");
+
+    /** Source English format. */
+    private final String sourceFormat;
+
+    /** Simple constructor.
+     * @param sourceFormat source English format to use when no
+     * localized version is available
+     */
+    LocalizedFilterFormats(final String sourceFormat) {
+        this.sourceFormat = sourceFormat;
+    }
+
+    /** {@inheritDoc} */
+    @Override
+    public String getSourceString() {
+        return sourceFormat;
+    }
+
+    /** {@inheritDoc} */
+    @Override
+    public String getLocalizedString(final Locale locale) {
+        return getLocalizedString("assets/" + LocalizedFilterFormats.class.getName().replaceAll("\\.", "/"),
+                                  name(), locale);
+    }
+
+}

hipparchus-filtering/src/main/java/org/hipparchus/filtering/kalman/AbstractKalmanFilter.java

@@ -38,13 +38,26 @@ public abstract class AbstractKalmanFilter<T extends Measurement> implements Kal
     /** Corrected state. */
     private ProcessEstimate corrected;
 
+    /** Prior corrected covariance. */
+    private RealMatrix priorCovariance;
+
+    /** State transition matrix. */
+    private RealMatrix stateTransitionMatrix;
+
+    /** Observer. */
+    private KalmanObserver observer;
+
+
     /** Simple constructor.
      * @param decomposer decomposer to use for the correction phase
      * @param initialState initial state
      */
     protected AbstractKalmanFilter(final MatrixDecomposer decomposer, final ProcessEstimate initialState) {
         this.decomposer = decomposer;
         this.corrected  = initialState;
+        this.priorCovariance = null;
+        this.stateTransitionMatrix = null;
+        this.observer = null;
     }
 
     /** Perform prediction step.
@@ -57,6 +70,8 @@ protected void predict(final double time, final RealVector predictedState, final
         final RealMatrix predictedCovariance =
                         stm.multiply(corrected.getCovariance().multiplyTransposed(stm)).add(noise);
         predicted = new ProcessEstimate(time, predictedState, predictedCovariance);
+        stateTransitionMatrix = stm;
+        priorCovariance = corrected.getCovariance();
         corrected = null;
     }
 
@@ -133,6 +148,21 @@ protected void correct(final T measurement, final RealMatrix stm, final RealVect
 
     }
 
+    /** Get the observer.
+     * @return the observer
+     */
+    protected KalmanObserver getObserver() {
+        return observer;
+    }
+
+
+    /** {@inheritDoc} */
+    @Override
+    public void setObserver(final KalmanObserver kalmanObserver) {
+        observer = kalmanObserver;
+        observer.init(this);
+    }
+
     /** Get the predicted state.
      * @return predicted state
      */
@@ -149,4 +179,9 @@ public ProcessEstimate getCorrected() {
         return corrected;
     }
 
+    /** {@inheritDoc} */
+    @Override
+    public RealMatrix getStateCrossCovariance() {
+        return priorCovariance.multiplyTransposed(stateTransitionMatrix);
+    }
 }

hipparchus-filtering/src/main/java/org/hipparchus/filtering/kalman/KalmanEstimate.java

@@ -0,0 +1,39 @@
+/*
+ * Licensed to the Hipparchus project under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The Hipparchus project 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
+ *
+ *      https://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.hipparchus.filtering.kalman;
+
+import org.hipparchus.linear.RealMatrix;
+
+public interface KalmanEstimate {
+
+    /** Get the current predicted state.
+     * @return current predicted state
+     */
+    ProcessEstimate getPredicted();
+
+    /** Get the current corrected state.
+     * @return current corrected state
+     */
+    ProcessEstimate getCorrected();
+
+    /** Get the cross-covariance between the previous state and the prediction.
+     * Not required for forward filtering, but required for the smoother.
+     * @return cross-covariance
+     */
+    RealMatrix getStateCrossCovariance();
+
+}

hipparchus-filtering/src/main/java/org/hipparchus/filtering/kalman/KalmanFilter.java

@@ -24,7 +24,7 @@
  * @param <T> the type of the measurements
  * @since 1.3
  */
-public interface KalmanFilter<T extends Measurement> {
+public interface KalmanFilter<T extends Measurement> extends KalmanEstimate {
 
     /** Perform one estimation step.
      * @param measurement single measurement to handle
@@ -34,14 +34,8 @@ public interface KalmanFilter<T extends Measurement> {
     ProcessEstimate estimationStep(T measurement)
         throws MathRuntimeException;
 
-    /** Get the current predicted state.
-     * @return current predicted state
+    /** Set the filter observer callback.
+     * @param observer the observer
      */
-    ProcessEstimate getPredicted();
-
-    /** Get the current corrected state.
-     * @return current corrected state
-     */
-    ProcessEstimate getCorrected();
-
+    void setObserver(KalmanObserver observer);
 }

hipparchus-filtering/src/main/java/org/hipparchus/filtering/kalman/KalmanObserver.java

@@ -0,0 +1,36 @@
+/*
+ * Licensed to the Hipparchus project under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The Hipparchus project 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
+ *
+ *      https://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.hipparchus.filtering.kalman;
+
+/** Observer for Kalman filter recursions.
+ * <p>
+ * This interface is intended to be implemented by users to monitor
+ * the progress of the Kalman filter estimator during estimation.
+ * </p>
+ */
+public interface KalmanObserver {
+
+    /** Callback for initialisation of observer.
+     * @param estimate estimate calculated by a Kalman filter
+     */
+    default void init(KalmanEstimate estimate) {}
+
+    /** Notification callback after each Kalman filter measurement update.
+     * @param estimate estimate calculated by a Kalman filter
+     */
+    void updatePerformed(KalmanEstimate estimate);
+}