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