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 import static org.junit.Assert.assertNotNull; 20 import static org.junit.Assert.assertTrue; 21 22 import androidx.test.filters.SmallTest; 23 24 import com.android.server.wifi.WifiBaseTest; 25 26 import org.junit.Test; 27 28 import java.util.Random; 29 30 /** 31 * Unit tests for {@link com.android.server.wifi.util.KalmanFilter}. 32 */ 33 @SmallTest 34 public class KalmanFilterTest extends WifiBaseTest { 35 /** 36 * Test that constructor works 37 */ 38 @Test testConstructor()39 public void testConstructor() throws Exception { 40 KalmanFilter kf = new KalmanFilter(); 41 assertNotNull(kf); 42 } 43 44 /** 45 * Sets up a Kalman filter to behave as as a phase-locked loop 46 * <p> 47 * Set up a 2-D model that generates sinusoidal output at a fixed frequency; the state 48 * transformation is just a rotation by a fixed angle, and the output matrix projects oen 49 * of the dimensions. 50 */ initializePll(double stepSizeRadians, double modelStandardDeviation, double measurementStandardDeviation)51 private KalmanFilter initializePll(double stepSizeRadians, 52 double modelStandardDeviation, 53 double measurementStandardDeviation) { 54 KalmanFilter kf = new KalmanFilter(); 55 double cos = Math.cos(stepSizeRadians); 56 double sin = Math.sin(stepSizeRadians); 57 kf.mF = new Matrix(2, new double[]{ 58 cos, sin, 59 -sin, cos}); 60 double modelVariance = modelStandardDeviation * modelStandardDeviation; 61 kf.mQ = new Matrix(2, new double[]{ 62 modelVariance, 0.0, 63 0.0, modelVariance}); 64 kf.mH = new Matrix(2, new double[]{1.0, 0.0}); 65 double measurementVariance = measurementStandardDeviation * measurementStandardDeviation; 66 kf.mR = new Matrix(1, new double[]{measurementVariance}); 67 double initialAPosterioriVariance = 10000.0; 68 kf.mP = new Matrix(2, new double[]{ 69 initialAPosterioriVariance, 0.0, 70 0.0, initialAPosterioriVariance}); 71 kf.mx = new Matrix(2, 1); 72 return kf; 73 } 74 75 private double mAmplitude = 10.0; 76 private double mStepSizeRadians = Math.PI / 17.01; 77 private int mTransitionPoint = 300; 78 79 /** 80 * Generates the ideal signal at time step i. 81 * <p> 82 * Sinusoid, with an abrupt phase shift thrown in to test transient response 83 */ idealSignal(int i)84 private double idealSignal(int i) { 85 double phi = mStepSizeRadians * i; 86 if (i > mTransitionPoint) { 87 phi = phi + Math.PI * .75; 88 } 89 return mAmplitude * Math.sin(phi); 90 } 91 92 private double mNoiseAmplitude = 3.0; 93 94 private int mSteps = 500; 95 private int mSeed = 271828; 96 97 /** 98 * Test that using the phase locked loop Kalman filter results in a residual that is 99 * a lot smaller than the noise in the signal. 100 */ 101 @Test testPhaseLockedLoop()102 public void testPhaseLockedLoop() throws Exception { 103 Random random = new Random(mSeed); 104 double modelStandardDeviation = 0.5; 105 double [] noise = new double[mSteps]; 106 for (int i = 0; i < mSteps; i++) { 107 noise[i] = random.nextGaussian() * mNoiseAmplitude; 108 } 109 double [] filtered = new double[mSteps]; 110 double [] residual = new double[mSteps]; 111 KalmanFilter kf = initializePll(mStepSizeRadians, modelStandardDeviation, mNoiseAmplitude); 112 for (int i = 0; i < mSteps; i++) { 113 kf.predict(); 114 kf.update(new Matrix(1, new double[] {idealSignal(i) + noise[i]})); 115 filtered[i] = kf.mx.get(0, 0); 116 residual[i] = filtered[i] - idealSignal(i); 117 } 118 double sum = 0.0; 119 double sumSquares = 0.0; 120 double n = 0.0; 121 for (int i = 0; i < mSteps; i++) { 122 if (i < 5 || (i > mTransitionPoint && i < mTransitionPoint + 20)) continue; 123 sum += residual[i]; 124 sumSquares += residual[i] * residual[i]; 125 n += 1.0; 126 } 127 double mean = sum / n; 128 double variance = (sumSquares - sum * sum) / (n * n); 129 assertTrue(mean < 0.1); 130 assertTrue(variance < 1.5); 131 assertNotNull(kf.toString()); 132 } 133 134 /** 135 * Test that the toString method works even if the matrices have not been set. 136 */ 137 @Test 138 public void testToStrinWithNullsInside() throws Exception { 139 assertNotNull(new KalmanFilter().toString()); 140 } 141 } 142