1 /* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 package com.android.server.wifi.util; 18 19 /** 20 * Utility providiing a basic Kalman filter 21 * 22 * For background, see https://en.wikipedia.org/wiki/Kalman_filter 23 */ 24 public class KalmanFilter { 25 public Matrix mF; // stateTransition 26 public Matrix mQ; // processNoiseCovariance 27 public Matrix mH; // observationModel 28 public Matrix mR; // observationNoiseCovariance 29 public Matrix mP; // aPosterioriErrorCovariance 30 public Matrix mx; // stateEstimate 31 32 /** 33 * Performs the prediction phase of the filter, using the state estimate to produce 34 * a new estimate for the current timestep. 35 */ predict()36 public void predict() { 37 mx = mF.dot(mx); 38 mP = mF.dot(mP).dotTranspose(mF).plus(mQ); 39 } 40 41 /** 42 * Updates the state estimate to incorporate the new observation z. 43 */ update(Matrix z)44 public void update(Matrix z) { 45 Matrix y = z.minus(mH.dot(mx)); 46 Matrix tS = mH.dot(mP).dotTranspose(mH).plus(mR); 47 Matrix tK = mP.dotTranspose(mH).dot(tS.inverse()); 48 mx = mx.plus(tK.dot(y)); 49 mP = mP.minus(tK.dot(mH).dot(mP)); 50 } 51 52 @Override toString()53 public String toString() { 54 return "{F: " + mF 55 + " Q: " + mQ 56 + " H: " + mH 57 + " R: " + mR 58 + " P: " + mP 59 + " x: " + mx 60 + "}"; 61 } 62 } 63