com/android/server/wifi/util/KalmanFilter.java

/*
 * Copyright (C) 2017 The Android Open Source Project
 *
 * Licensed 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 com.android.server.wifi.util;

/**
 * Utility providiing a basic Kalman filter
 *
 * For background, see https://en.wikipedia.org/wiki/Kalman_filter
 */
public class KalmanFilter {
    public Matrix mF; // stateTransition
    public Matrix mQ; // processNoiseCovariance
    public Matrix mH; // observationModel
    public Matrix mR; // observationNoiseCovariance
    public Matrix mP; // aPosterioriErrorCovariance
    public Matrix mx; // stateEstimate

    /**
     * Performs the prediction phase of the filter, using the state estimate to produce
     * a new estimate for the current timestep.
     */
    public void predict() {
        mx = mF.dot(mx);
        mP = mF.dot(mP).dotTranspose(mF).plus(mQ);
    }

    /**
     * Updates the state estimate to incorporate the new observation z.
     */
    public void update(Matrix z) {
        Matrix y = z.minus(mH.dot(mx));
        Matrix tS = mH.dot(mP).dotTranspose(mH).plus(mR);
        Matrix tK = mP.dotTranspose(mH).dot(tS.inverse());
        mx = mx.plus(tK.dot(y));
        mP = mP.minus(tK.dot(mH).dot(mP));
    }

    @Override
    public String toString() {
        return "{F: " + mF
                + " Q: " + mQ
                + " H: " + mH
                + " R: " + mR
                + " P: " + mP
                + " x: " + mx
                + "}";
    }
}