LinearKalmanFilter.java

/*
 * 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.linear;

import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.filtering.kalman.AbstractKalmanFilter;
import org.hipparchus.filtering.kalman.Measurement;
import org.hipparchus.filtering.kalman.ProcessEstimate;
import org.hipparchus.linear.MatrixDecomposer;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;

/**
 * Kalman filter for {@link LinearProcess linear process}.
 * @param <T> the type of the measurements
 * @since 1.3
 */
public class LinearKalmanFilter<T extends Measurement> extends AbstractKalmanFilter<T> {

    /** Process to be estimated. */
    private final LinearProcess<T> process;

    /** Simple constructor.
     * @param decomposer decomposer to use for the correction phase
     * @param process linear process to estimate
     * @param initialState initial state
     */
    public LinearKalmanFilter(final MatrixDecomposer decomposer,
                              final LinearProcess<T> process,
                              final ProcessEstimate initialState) {
        super(decomposer, initialState);
        this.process = process;
    }

    /** {@inheritDoc} */
    @Override
    public ProcessEstimate estimationStep(final T measurement)
        throws MathRuntimeException {

        final LinearEvolution evolution = process.getEvolution(measurement);

        // prediction phase
        final RealMatrix      a         = evolution.getStateTransitionMatrix();
        final RealMatrix      b         = evolution.getControlMatrix();
        final RealVector      u         = (b == null) ? null : evolution.getCommand();
        final RealMatrix      q         = evolution.getProcessNoiseMatrix();

        RealVector predXk = a.operate(getCorrected().getState());
        if (b != null) {
            predXk = predXk.add(b.operate(u));
        }

        predict(measurement.getTime(), predXk, a, q);

        // correction phase
        final RealMatrix h          = evolution.getMeasurementJacobian();
        final RealMatrix s          = computeInnovationCovarianceMatrix(measurement.getCovariance(), h);
        final RealVector innovation = (h == null) ? null : measurement.getValue().subtract(h.operate(predXk));
        correct(measurement, a, innovation, h, s);

        if (getObserver() != null) {
            getObserver().updatePerformed(this);
        }

        return getCorrected();

    }

}