content: /*
   * 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
                  + "}";
      }
  }
  