├── README.md ├── LICENSE └── src └── com └── androzic └── utils ├── KalmanFilter.java ├── GeoTrackFilter.java └── Matrix.java /README.md: -------------------------------------------------------------------------------- 1 | GeoTrackFilter 2 | ============== 3 | 4 | Implementation of Kalman filter for geo (gps) tracks. This is a Java port of 5 | original C code by Kevin Lacker: https://github.com/lacker/ikalman 6 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | (The MIT License.) 2 | 3 | Copyright (c) 2009, Kevin Lacker. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. -------------------------------------------------------------------------------- /src/com/androzic/utils/KalmanFilter.java: -------------------------------------------------------------------------------- 1 | /* 2 | (The MIT License.) 3 | 4 | Copyright (c) 2009, Kevin Lacker. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | /* 26 | * This is a Java port of original C code by Kevin Lacker. 27 | * https://github.com/lacker/ikalman 28 | * 29 | * Ported by Andrey Novikov, 2013 30 | */ 31 | 32 | package com.androzic.utils; 33 | 34 | /* Refer to http://en.wikipedia.org/wiki/Kalman_filter for 35 | mathematical details. The naming scheme is that variables get names 36 | that make sense, and are commented with their analog in 37 | the Wikipedia mathematical notation. 38 | This Kalman filter implementation does not support controlled 39 | input. 40 | (Like knowing which way the steering wheel in a car is turned and 41 | using that to inform the model.) 42 | Vectors are handled as n-by-1 matrices. 43 | TODO: comment on the dimension of the matrices */ 44 | 45 | public class KalmanFilter { 46 | /* k */ 47 | int timestep; 48 | 49 | /* These parameters define the size of the matrices. */ 50 | int state_dimension, observation_dimension; 51 | 52 | /* This group of matrices must be specified by the user. */ 53 | /* F_k */ 54 | Matrix state_transition; 55 | /* H_k */ 56 | Matrix observation_model; 57 | /* Q_k */ 58 | Matrix process_noise_covariance; 59 | /* R_k */ 60 | Matrix observation_noise_covariance; 61 | 62 | /* The observation is modified by the user before every time step. */ 63 | /* z_k */ 64 | Matrix observation; 65 | 66 | /* This group of matrices are updated every time step by the filter. */ 67 | /* x-hat_k|k-1 */ 68 | Matrix predicted_state; 69 | /* P_k|k-1 */ 70 | Matrix predicted_estimate_covariance; 71 | /* y-tilde_k */ 72 | Matrix innovation; 73 | /* S_k */ 74 | Matrix innovation_covariance; 75 | /* S_k^-1 */ 76 | Matrix inverse_innovation_covariance; 77 | /* K_k */ 78 | Matrix optimal_gain; 79 | /* x-hat_k|k */ 80 | Matrix state_estimate; 81 | /* P_k|k */ 82 | Matrix estimate_covariance; 83 | 84 | /* This group is used for meaningless intermediate calculations */ 85 | Matrix vertical_scratch; 86 | Matrix small_square_scratch; 87 | Matrix big_square_scratch; 88 | 89 | public KalmanFilter(int state_dimension, int observation_dimension) { 90 | timestep = 0; 91 | this.state_dimension = state_dimension; 92 | this.observation_dimension = observation_dimension; 93 | 94 | state_transition = new Matrix(state_dimension, state_dimension); 95 | observation_model = new Matrix(observation_dimension, state_dimension); 96 | process_noise_covariance = new Matrix(state_dimension, state_dimension); 97 | observation_noise_covariance = new Matrix(observation_dimension, observation_dimension); 98 | 99 | observation = new Matrix(observation_dimension, 1); 100 | 101 | predicted_state = new Matrix(state_dimension, 1); 102 | predicted_estimate_covariance = new Matrix(state_dimension, state_dimension); 103 | innovation = new Matrix(observation_dimension, 1); 104 | innovation_covariance = new Matrix(observation_dimension, observation_dimension); 105 | inverse_innovation_covariance = new Matrix(observation_dimension, observation_dimension); 106 | optimal_gain = new Matrix(state_dimension, observation_dimension); 107 | state_estimate = new Matrix(state_dimension, 1); 108 | estimate_covariance = new Matrix(state_dimension, state_dimension); 109 | 110 | vertical_scratch = new Matrix(state_dimension, observation_dimension); 111 | small_square_scratch = new Matrix(observation_dimension,observation_dimension); 112 | big_square_scratch = new Matrix(state_dimension, state_dimension); 113 | } 114 | 115 | /* 116 | * Runs one timestep of prediction + estimation. 117 | * 118 | * Before each time step of running this, set f.observation to be the next 119 | * time step's observation. 120 | * 121 | * Before the first step, define the model by setting: f.state_transition 122 | * f.observation_model f.process_noise_covariance 123 | * f.observation_noise_covariance 124 | * 125 | * It is also advisable to initialize with reasonable guesses for 126 | * f.state_estimate f.estimate_covariance 127 | */ 128 | void update() { 129 | predict(); 130 | estimate(); 131 | } 132 | 133 | /* Just the prediction phase of update. */ 134 | void predict() { 135 | timestep++; 136 | 137 | /* Predict the state */ 138 | Matrix.multiply_matrix(state_transition, state_estimate, predicted_state); 139 | 140 | /* Predict the state estimate covariance */ 141 | Matrix.multiply_matrix(state_transition, estimate_covariance, big_square_scratch); 142 | Matrix.multiply_by_transpose_matrix(big_square_scratch, state_transition, predicted_estimate_covariance); 143 | Matrix.add_matrix(predicted_estimate_covariance, process_noise_covariance, predicted_estimate_covariance); 144 | } 145 | 146 | /* Just the estimation phase of update. */ 147 | void estimate() { 148 | /* Calculate innovation */ 149 | Matrix.multiply_matrix(observation_model, predicted_state, innovation); 150 | Matrix.subtract_matrix(observation, innovation, innovation); 151 | 152 | /* Calculate innovation covariance */ 153 | Matrix.multiply_by_transpose_matrix(predicted_estimate_covariance, observation_model, vertical_scratch); 154 | Matrix.multiply_matrix(observation_model, vertical_scratch, innovation_covariance); 155 | Matrix.add_matrix(innovation_covariance, observation_noise_covariance, innovation_covariance); 156 | 157 | /* 158 | * Invert the innovation covariance. Note: this destroys the innovation 159 | * covariance. TODO: handle inversion failure intelligently. 160 | */ 161 | Matrix.destructive_invert_matrix(innovation_covariance, inverse_innovation_covariance); 162 | 163 | /* 164 | * Calculate the optimal Kalman gain. Note we still have a useful 165 | * partial product in vertical scratch from the innovation covariance. 166 | */ 167 | Matrix.multiply_matrix(vertical_scratch, inverse_innovation_covariance, optimal_gain); 168 | 169 | /* Estimate the state */ 170 | Matrix.multiply_matrix(optimal_gain, innovation, state_estimate); 171 | Matrix.add_matrix(state_estimate, predicted_state, state_estimate); 172 | 173 | /* Estimate the state covariance */ 174 | Matrix.multiply_matrix(optimal_gain, observation_model, big_square_scratch); 175 | Matrix.subtract_from_identity_matrix(big_square_scratch); 176 | Matrix.multiply_matrix(big_square_scratch, predicted_estimate_covariance, estimate_covariance); 177 | } 178 | } 179 | -------------------------------------------------------------------------------- /src/com/androzic/utils/GeoTrackFilter.java: -------------------------------------------------------------------------------- 1 | /* 2 | (The MIT License.) 3 | 4 | Copyright (c) 2009, Kevin Lacker. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | /* 26 | * This is a Java port of original C code by Kevin Lacker. 27 | * https://github.com/lacker/ikalman 28 | * 29 | * Ported by Andrey Novikov, 2013 30 | */ 31 | 32 | package com.androzic.utils; 33 | 34 | /* To use these functions: 35 | 36 | 1. Start with a KalmanFilter created by alloc_filter_velocity2d. 37 | 2. At fixed intervals, call update_velocity2d with the lat/long. 38 | 3. At any time, to get an estimate for the current position, 39 | bearing, or speed, use the functions: 40 | get_lat_long 41 | get_bearing 42 | get_mph 43 | */ 44 | 45 | public class GeoTrackFilter { 46 | // FIXME Radius should be calculated depending on latitude instead 47 | // http://en.wikipedia.org/wiki/Earth_radius#Radius_at_a_given_geodetic_latitude 48 | private static final double EARTH_RADIUS_IN_METERS = 6371009; 49 | 50 | private KalmanFilter f; 51 | 52 | /* 53 | * Create a GPS filter that only tracks two dimensions of position and 54 | * velocity. The inherent assumption is that changes in velocity are 55 | * randomly distributed around 0. Noise is a parameter you can use to alter 56 | * the expected noise. 1.0 is the original, and the higher it is, the more a 57 | * path will be "smoothed". Free with free_filter after using. 58 | */ 59 | 60 | GeoTrackFilter(double noise) { 61 | /* 62 | * The state model has four dimensions: x, y, x', y' Each time step we 63 | * can only observe position, not velocity, so the observation vector 64 | * has only two dimensions. 65 | */ 66 | f = new KalmanFilter(4, 2); 67 | 68 | /* 69 | * Assuming the axes are rectilinear does not work well at the poles, 70 | * but it has the bonus that we don't need to convert between lat/long 71 | * and more rectangular coordinates. The slight inaccuracy of our 72 | * physics model is not too important. 73 | */ 74 | f.state_transition.set_identity_matrix(); 75 | set_seconds_per_timestep(1.0); 76 | 77 | /* We observe (x, y) in each time step */ 78 | f.observation_model.set_matrix(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); 79 | 80 | /* Noise in the world. */ 81 | double pos = 0.000001; 82 | f.process_noise_covariance.set_matrix(pos, 0.0, 0.0, 0.0, 0.0, pos, 83 | 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0); 84 | 85 | /* Noise in our observation */ 86 | f.observation_noise_covariance.set_matrix(pos * noise, 0.0, 0.0, pos 87 | * noise); 88 | 89 | /* The start position is totally unknown, so give a high variance */ 90 | f.state_estimate.set_matrix(0.0, 0.0, 0.0, 0.0); 91 | f.estimate_covariance.set_identity_matrix(); 92 | double trillion = 1000.0 * 1000.0 * 1000.0 * 1000.0; 93 | f.estimate_covariance.scale_matrix(trillion); 94 | } 95 | 96 | /* Set the seconds per timestep in the velocity2d model. */ 97 | /* 98 | * The position units are in thousandths of latitude and longitude. The 99 | * velocity units are in thousandths of position units per second. 100 | * 101 | * So if there is one second per timestep, a velocity of 1 will change the 102 | * lat or long by 1 after a million timesteps. 103 | * 104 | * Thus a typical position is hundreds of thousands of units. A typical 105 | * velocity is maybe ten. 106 | */ 107 | void set_seconds_per_timestep(double seconds_per_timestep) { 108 | /* 109 | * unit_scaler accounts for the relation between position and velocity 110 | * units 111 | */ 112 | double unit_scaler = 0.001; 113 | f.state_transition.data[0][2] = unit_scaler * seconds_per_timestep; 114 | f.state_transition.data[1][3] = unit_scaler * seconds_per_timestep; 115 | } 116 | 117 | /* Update the velocity2d model with new gps data. */ 118 | void update_velocity2d(double lat, double lon, 119 | double seconds_since_last_timestep) { 120 | set_seconds_per_timestep(seconds_since_last_timestep); 121 | f.observation.set_matrix(lat * 1000.0, lon * 1000.0); 122 | f.update(); 123 | } 124 | 125 | /* Extract a lat long from a velocity2d Kalman filter. */ 126 | double[] get_lat_long() { 127 | double[] latlon = new double[2]; 128 | latlon[0] = f.state_estimate.data[0][0] / 1000.0; 129 | latlon[1] = f.state_estimate.data[1][0] / 1000.0; 130 | return latlon; 131 | } 132 | 133 | /* 134 | * Extract velocity with lat-long-per-second units from a velocity2d Kalman 135 | * filter. 136 | */ 137 | double[] get_velocity() { 138 | double[] delta_latlon = new double[2]; 139 | delta_latlon[0] = f.state_estimate.data[2][0] / (1000.0 * 1000.0); 140 | delta_latlon[1] = f.state_estimate.data[3][0] / (1000.0 * 1000.0); 141 | return delta_latlon; 142 | } 143 | 144 | /* 145 | * Extract a bearing from a velocity2d Kalman filter. 0 = north, 90 = east, 146 | * 180 = south, 270 = west 147 | */ 148 | /* 149 | * See http://www.movable-type.co.uk/scripts/latlong.html for formulas 150 | */ 151 | double get_bearing() { 152 | double x, y; 153 | double[] latlon = get_lat_long(); 154 | double[] delta_latlon = get_velocity(); 155 | 156 | /* Convert to radians */ 157 | latlon[0] = Math.toRadians(latlon[0]); 158 | latlon[1] = Math.toRadians(latlon[1]); 159 | delta_latlon[0] = Math.toRadians(delta_latlon[0]); 160 | delta_latlon[1] = Math.toRadians(delta_latlon[1]); 161 | 162 | /* Do math */ 163 | double lat1 = latlon[0] - delta_latlon[0]; 164 | y = Math.sin(delta_latlon[1]) * Math.cos(latlon[0]); 165 | x = Math.cos(lat1) * Math.sin(latlon[0]) - Math.sin(lat1) 166 | * Math.cos(latlon[0]) * Math.cos(delta_latlon[1]); 167 | double bearing = Math.atan2(y, x); 168 | 169 | /* Convert to degrees */ 170 | bearing = Math.toDegrees(bearing); 171 | return bearing; 172 | } 173 | 174 | /* Extract speed in meters per second from a velocity2d Kalman filter. */ 175 | double get_speed(double altitude) { 176 | double[] latlon = get_lat_long(); 177 | double[] delta_latlon = get_velocity(); 178 | /* 179 | * First, let's calculate a unit-independent measurement - the radii of 180 | * the earth traveled in each second. (Presumably this will be a very 181 | * small number.) 182 | */ 183 | 184 | /* Convert to radians */ 185 | latlon[0] = Math.toRadians(latlon[0]); 186 | latlon[1] = Math.toRadians(latlon[1]); 187 | delta_latlon[0] = Math.toRadians(delta_latlon[0]); 188 | delta_latlon[1] = Math.toRadians(delta_latlon[1]); 189 | 190 | /* Haversine formula */ 191 | double lat1 = latlon[0] - delta_latlon[0]; 192 | double sin_half_dlat = Math.sin(delta_latlon[0] / 2.0); 193 | double sin_half_dlon = Math.sin(delta_latlon[1] / 2.0); 194 | double a = sin_half_dlat * sin_half_dlat + Math.cos(lat1) * Math.cos(latlon[0]) * sin_half_dlon * sin_half_dlon; 195 | double radians_per_second = 2 * Math.atan2(1000.0 * Math.sqrt(a), 1000.0 * Math.sqrt(1.0 - a)); 196 | 197 | /* Convert units */ 198 | double meters_per_second = radians_per_second * (EARTH_RADIUS_IN_METERS + altitude); 199 | return meters_per_second; 200 | } 201 | 202 | public static void main(String[] args) 203 | { 204 | } 205 | } 206 | -------------------------------------------------------------------------------- /src/com/androzic/utils/Matrix.java: -------------------------------------------------------------------------------- 1 | /* 2 | (The MIT License.) 3 | 4 | Copyright (c) 2009, Kevin Lacker. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | /* 26 | * This is a Java port of original C code by Kevin Lacker. 27 | * https://github.com/lacker/ikalman 28 | * 29 | * Ported by Andrey Novikov, 2013 30 | */ 31 | 32 | package com.androzic.utils; 33 | 34 | public class Matrix { 35 | /* Dimensions */ 36 | int rows; 37 | int cols; 38 | 39 | /* Contents of the matrix */ 40 | double[][] data; 41 | 42 | /* 43 | * Allocate memory for a new matrix. Zeros out the matrix. Assert-fails if 44 | * we are out of memory. 45 | */ 46 | Matrix(int rows, int cols) { 47 | this.rows = rows; 48 | this.cols = cols; 49 | this.data = new double[rows][cols]; 50 | } 51 | 52 | /* Set values of a matrix, row by row. */ 53 | void set_matrix(double... arg) { 54 | assert (arg.length == rows * cols); 55 | for (int i = 0; i < rows; i++) { 56 | for (int j = 0; j < cols; j++) { 57 | data[i][j] = arg[i * cols + j]; 58 | } 59 | } 60 | } 61 | 62 | /* Turn m into an identity matrix. */ 63 | void set_identity_matrix() { 64 | assert (rows == cols); 65 | for (int i = 0; i < rows; ++i) { 66 | for (int j = 0; j < cols; ++j) { 67 | if (i == j) { 68 | data[i][j] = 1.0; 69 | } else { 70 | data[i][j] = 0.0; 71 | } 72 | } 73 | } 74 | } 75 | 76 | /* Copy a matrix. */ 77 | static void copy_matrix(Matrix source, Matrix destination) { 78 | assert (source.rows == destination.rows); 79 | assert (source.cols == destination.cols); 80 | for (int i = 0; i < source.rows; ++i) { 81 | for (int j = 0; j < source.cols; ++j) { 82 | destination.data[i][j] = source.data[i][j]; 83 | } 84 | } 85 | } 86 | 87 | /* Pretty-print a matrix. */ 88 | void print_matrix() { 89 | for (int i = 0; i < rows; ++i) { 90 | for (int j = 0; j < cols; ++j) { 91 | if (j > 0) 92 | System.out.print(" "); 93 | System.out.format("%6.2f", data[i][j]); 94 | } 95 | System.out.print("\n"); 96 | } 97 | } 98 | 99 | /* Add matrices a and b and put the result in c. */ 100 | static void add_matrix(Matrix a, Matrix b, Matrix c) { 101 | assert (a.rows == b.rows); 102 | assert (a.rows == c.rows); 103 | assert (a.cols == b.cols); 104 | assert (a.cols == c.cols); 105 | for (int i = 0; i < a.rows; ++i) { 106 | for (int j = 0; j < a.cols; ++j) { 107 | c.data[i][j] = a.data[i][j] + b.data[i][j]; 108 | } 109 | } 110 | } 111 | 112 | /* Subtract matrices a and b and put the result in c. */ 113 | static void subtract_matrix(Matrix a, Matrix b, Matrix c) { 114 | assert (a.rows == b.rows); 115 | assert (a.rows == c.rows); 116 | assert (a.cols == b.cols); 117 | assert (a.cols == c.cols); 118 | for (int i = 0; i < a.rows; ++i) { 119 | for (int j = 0; j < a.cols; ++j) { 120 | c.data[i][j] = a.data[i][j] - b.data[i][j]; 121 | } 122 | } 123 | } 124 | 125 | /* Subtract from the identity matrix in place. */ 126 | static void subtract_from_identity_matrix(Matrix a) { 127 | assert (a.rows == a.cols); 128 | for (int i = 0; i < a.rows; ++i) { 129 | for (int j = 0; j < a.cols; ++j) { 130 | if (i == j) { 131 | a.data[i][j] = 1.0 - a.data[i][j]; 132 | } else { 133 | a.data[i][j] = 0.0 - a.data[i][j]; 134 | } 135 | } 136 | } 137 | } 138 | 139 | /* Multiply matrices a and b and put the result in c. */ 140 | static void multiply_matrix(Matrix a, Matrix b, Matrix c) { 141 | assert (a.cols == b.rows); 142 | assert (a.rows == c.rows); 143 | assert (b.cols == c.cols); 144 | for (int i = 0; i < c.rows; ++i) { 145 | for (int j = 0; j < c.cols; ++j) { 146 | /* 147 | * Calculate element c.data[i][j] via a dot product of one row 148 | * of a with one column of b 149 | */ 150 | c.data[i][j] = 0.0; 151 | for (int k = 0; k < a.cols; ++k) { 152 | c.data[i][j] += a.data[i][k] * b.data[k][j]; 153 | } 154 | } 155 | } 156 | } 157 | 158 | /* Multiply matrix a by b-transpose and put the result in c. */ 159 | /* 160 | * This is multiplying a by b-tranpose so it is like multiply_matrix but 161 | * references to b reverse rows and cols. 162 | */ 163 | static void multiply_by_transpose_matrix(Matrix a, Matrix b, Matrix c) { 164 | assert (a.cols == b.cols); 165 | assert (a.rows == c.rows); 166 | assert (b.rows == c.cols); 167 | for (int i = 0; i < c.rows; ++i) { 168 | for (int j = 0; j < c.cols; ++j) { 169 | /* 170 | * Calculate element c.data[i][j] via a dot product of one row 171 | * of a with one row of b 172 | */ 173 | c.data[i][j] = 0.0; 174 | for (int k = 0; k < a.cols; ++k) { 175 | c.data[i][j] += a.data[i][k] * b.data[j][k]; 176 | } 177 | } 178 | } 179 | } 180 | 181 | /* Transpose input and put the result in output. */ 182 | static void transpose_matrix(Matrix input, Matrix output) { 183 | assert (input.rows == output.cols); 184 | assert (input.cols == output.rows); 185 | for (int i = 0; i < input.rows; ++i) { 186 | for (int j = 0; j < input.cols; ++j) { 187 | output.data[j][i] = input.data[i][j]; 188 | } 189 | } 190 | } 191 | 192 | /* Whether two matrices are approximately equal. */ 193 | static boolean equal_matrix(Matrix a, Matrix b, double tolerance) { 194 | assert (a.rows == b.rows); 195 | assert (a.cols == b.cols); 196 | for (int i = 0; i < a.rows; ++i) { 197 | for (int j = 0; j < a.cols; ++j) { 198 | if (Math.abs(a.data[i][j] - b.data[i][j]) > tolerance) 199 | return false; 200 | } 201 | } 202 | return true; 203 | } 204 | 205 | /* Multiply a matrix by a scalar. */ 206 | void scale_matrix(double scalar) { 207 | assert (scalar != 0.0); 208 | for (int i = 0; i < rows; ++i) { 209 | for (int j = 0; j < cols; ++j) { 210 | data[i][j] *= scalar; 211 | } 212 | } 213 | } 214 | 215 | /* 216 | * Swap rows r1 and r2 of a matrix. This is one of the three 217 | * "elementary row operations". 218 | */ 219 | void swap_rows(int r1, int r2) { 220 | assert (r1 != r2); 221 | double[] tmp = data[r1]; 222 | data[r1] = data[r2]; 223 | data[r2] = tmp; 224 | } 225 | 226 | /* 227 | * Multiply row r of a matrix by a scalar. This is one of the three 228 | * "elementary row operations". 229 | */ 230 | void scale_row(int r, double scalar) { 231 | assert (scalar != 0.0); 232 | for (int i = 0; i < cols; ++i) 233 | data[r][i] *= scalar; 234 | } 235 | 236 | /* 237 | * Add a multiple of row r2 to row r1. Also known as a "shear" operation. 238 | * This is one of the three "elementary row operations". 239 | */ 240 | /* Add scalar * row r2 to row r1. */ 241 | void shear_row(int r1, int r2, double scalar) { 242 | assert (r1 != r2); 243 | for (int i = 0; i < cols; ++i) 244 | data[r1][i] += scalar * data[r2][i]; 245 | } 246 | 247 | /* 248 | * Invert a square matrix. Returns whether the matrix is invertible. input 249 | * is mutated as well by this routine. 250 | */ 251 | 252 | /* 253 | * Uses Gauss-Jordan elimination. 254 | * 255 | * The elimination procedure works by applying elementary row operations to 256 | * our input matrix until the input matrix is reduced to the identity 257 | * matrix. Simultaneously, we apply the same elementary row operations to a 258 | * separate identity matrix to produce the inverse matrix. If this makes no 259 | * sense, read wikipedia on Gauss-Jordan elimination. 260 | * 261 | * This is not the fastest way to invert matrices, so this is quite possibly 262 | * the bottleneck. 263 | */ 264 | static boolean destructive_invert_matrix(Matrix input, Matrix output) { 265 | assert (input.rows == input.cols); 266 | assert (input.rows == output.rows); 267 | assert (input.rows == output.cols); 268 | 269 | output.set_identity_matrix(); 270 | 271 | /* 272 | * Convert input to the identity matrix via elementary row operations. 273 | * The ith pass through this loop turns the element at i,i to a 1 and 274 | * turns all other elements in column i to a 0. 275 | */ 276 | for (int i = 0; i < input.rows; ++i) { 277 | if (input.data[i][i] == 0.0) { 278 | /* We must swap rows to get a nonzero diagonal element. */ 279 | int r; 280 | for (r = i + 1; r < input.rows; ++r) { 281 | if (input.data[r][i] != 0.0) 282 | break; 283 | } 284 | if (r == input.rows) { 285 | /* 286 | * Every remaining element in this column is zero, so this 287 | * matrix cannot be inverted. 288 | */ 289 | return false; 290 | } 291 | input.swap_rows(i, r); 292 | output.swap_rows(i, r); 293 | } 294 | 295 | /* 296 | * Scale this row to ensure a 1 along the diagonal. We might need to 297 | * worry about overflow from a huge scalar here. 298 | */ 299 | double scalar = 1.0 / input.data[i][i]; 300 | input.scale_row(i, scalar); 301 | output.scale_row(i, scalar); 302 | 303 | /* Zero out the other elements in this column. */ 304 | for (int j = 0; j < input.rows; ++j) { 305 | if (i == j) 306 | continue; 307 | double shear_needed = -input.data[j][i]; 308 | input.shear_row(j, i, shear_needed); 309 | output.shear_row(j, i, shear_needed); 310 | } 311 | } 312 | 313 | return true; 314 | } 315 | } 316 | --------------------------------------------------------------------------------