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 android.location.cts.gnss.pseudorange; 18 19 20 /** 21 * Helper class containing the basic vector and matrix operations used for calculating the position 22 * solution from pseudoranges 23 * TODO: use standard matrix library to replace the operations in this class. 24 * 25 */ 26 public class GpsMathOperations { 27 28 /** 29 * Calculates the norm of a vector 30 */ vectorNorm(double[] inputVector)31 public static double vectorNorm(double[] inputVector) { 32 double normSqured = 0; 33 for (int i = 0; i < inputVector.length; i++) { 34 normSqured = Math.pow(inputVector[i], 2) + normSqured; 35 } 36 37 return Math.sqrt(normSqured); 38 } 39 40 /** 41 * Subtract two vectors {@code firstVector} - {@code secondVector}. Both vectors should be of the 42 * same length. 43 */ subtractTwoVectors(double[] firstVector, double[] secondVector)44 public static double[] subtractTwoVectors(double[] firstVector, double[] secondVector) 45 throws ArithmeticException { 46 double[] result = new double[firstVector.length]; 47 if (firstVector.length != secondVector.length) { 48 throw new ArithmeticException("Input vectors are of different lengths"); 49 } 50 51 for (int i = 0; i < firstVector.length; i++) { 52 result[i] = firstVector[i] - secondVector[i]; 53 } 54 55 return result; 56 } 57 58 /** 59 * Multiply a matrix {@code matrix} by a column vector {@code vector} 60 * ({@code matrix} * {@code vector}) and return the resulting vector {@resultVector}. 61 * {@code matrix} and {@vector} dimensions must match. 62 */ matrixByColVectMultiplication(double[][] matrix, double[] vector)63 public static double[] matrixByColVectMultiplication(double[][] matrix, double[] vector) 64 throws ArithmeticException { 65 double result[] = new double[matrix.length]; 66 int matrixLength = matrix.length; 67 int vectorLength = vector.length; 68 if (vectorLength != matrix[0].length) { 69 throw new ArithmeticException("Matrix and vector dimensions do not match"); 70 } 71 72 for (int i = 0; i < matrixLength; i++) { 73 for (int j = 0; j < vectorLength; j++) { 74 result[i] += matrix[i][j] * vector[j]; 75 } 76 } 77 78 return result; 79 } 80 81 /** 82 * Dot product of a raw vector {@code firstVector} and a column vector {@code secondVector}. 83 * Both vectors should be of the same length. 84 */ dotProduct(double[] firstVector, double[] secondVector)85 public static double dotProduct(double[] firstVector, double[] secondVector) 86 throws ArithmeticException { 87 if (firstVector.length != secondVector.length) { 88 throw new ArithmeticException("Input vectors are of different lengths"); 89 } 90 double result = 0; 91 for (int i = 0; i < firstVector.length; i++) { 92 result = firstVector[i] * secondVector[i] + result; 93 } 94 return result; 95 } 96 97 } 98