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