1 /*
2 * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
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 package com.irurueta.numerical.signal.processing;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.ArrayUtils;
20 import com.irurueta.algebra.Matrix;
21
22 /**
23 * Estimates noise covariance matrix for a given set of measures.
24 * Covariance matrix is updated each time that measures are added.
25 * This class also computes average values of samples to estimate the bias
26 * on given noise samples.
27 * This class should only be used for samples obtained while system state is
28 * held constant.
29 */
30 public class MeasurementNoiseCovarianceEstimator {
31
32 /**
33 * Number of measurement vector dimensions (measure parameters).
34 */
35 private final int mp;
36
37 /**
38 * Estimated measurement noise covariance matrix.
39 */
40 private final Matrix measurementNoiseCov;
41
42 /**
43 * Estimated sample average.
44 */
45 private final double[] sampleAverage;
46
47 /**
48 * Number of samples used for estimation.
49 */
50 private long sampleCount;
51
52 /**
53 * A sample after removing its mean. This is used internally, and it is
54 * kept as an instance variable for reuse purposes.
55 */
56 private final double[] sampleNoMean;
57
58 /**
59 * A sample expressed in matrix form. This is used internally, and it is kept
60 * as an instance variable for reuse purposes.
61 */
62 private final Matrix sampleMatrix;
63
64 /**
65 * The transposed sample matrix. This is used internally, and it is kept as
66 * an instance variable for reuse purposes.
67 */
68 private final Matrix transposedSampleMatrix;
69
70 /**
71 * A covariance matrix for a single sample. This is used internally, and it
72 * is kept as an instance variable for reuse purposes.
73 */
74 private final Matrix singleCov;
75
76 /**
77 * Constructor.
78 *
79 * @param measureParams number of measurement parameters for each sample
80 * (i.e. when sampling 3D acceleration samples, this value must be 3, since
81 * each sample contains acceleration values for x, y, and z axes).
82 * @throws IllegalArgumentException if provided number of measure parameters
83 * is less than 1.
84 * @throws SignalProcessingException if something fails.
85 */
86 public MeasurementNoiseCovarianceEstimator(final int measureParams) throws SignalProcessingException {
87
88 if (measureParams < 1) {
89 throw new IllegalArgumentException("Measure parameters must be greater than zero");
90 }
91
92 try {
93 mp = measureParams;
94 measurementNoiseCov = new Matrix(mp, mp);
95 sampleAverage = new double[mp];
96
97 sampleNoMean = new double[mp];
98 sampleMatrix = new Matrix(mp, 1);
99 transposedSampleMatrix = new Matrix(1, mp);
100
101 singleCov = new Matrix(mp, mp);
102 } catch (final AlgebraException ex) {
103 throw new SignalProcessingException(ex);
104 }
105 }
106
107 /**
108 * Updates currently estimated covariance matrix by adding provided sample
109 * data.
110 *
111 * @param sample sample to be added to update covariance matrix.
112 * @return covariance matrix after update.
113 * @throws IllegalArgumentException if provided sample length is not equal
114 * to the number of measure parameters set for this instance.
115 * @throws SignalProcessingException if something fails.
116 */
117 public Matrix update(final double[] sample) throws SignalProcessingException {
118
119 if (sample.length != mp) {
120 throw new IllegalArgumentException("wrong sample size");
121 }
122
123 final var nextCount = sampleCount + 1;
124
125 // update sample average
126 for (int i = 0; i < mp; i++) {
127 sampleAverage[i] = (sampleAverage[i] * sampleCount + sample[i]) / nextCount;
128 }
129
130 // compute sample without mean
131 ArrayUtils.subtract(sample, sampleAverage, sampleNoMean);
132
133 // copy sample without mean into matrix form
134 sampleMatrix.setSubmatrix(0, 0, mp - 1, 0, sampleNoMean);
135 // compute transpose of matrix form
136 sampleMatrix.transpose(transposedSampleMatrix);
137
138 try {
139 // compute covariance matrix for a single sample
140 sampleMatrix.multiply(transposedSampleMatrix, singleCov);
141
142 // update covariance matrix
143 measurementNoiseCov.multiplyByScalar(sampleCount);
144 measurementNoiseCov.add(singleCov);
145 measurementNoiseCov.multiplyByScalar(1.0 / nextCount);
146 } catch (final AlgebraException ex) {
147 throw new SignalProcessingException(ex);
148 }
149
150 sampleCount = nextCount;
151
152 return measurementNoiseCov;
153 }
154
155 /**
156 * Obtains the number of measurement vector dimensions (measure parameters).
157 *
158 * @return number of measurement vector dimensions (measure parameters).
159 */
160 public int getMeasureParams() {
161 return mp;
162 }
163
164 /**
165 * Obtains estimated measurement noise covariance matrix.
166 *
167 * @return estimated measurement noise covariance matrix.
168 */
169 public Matrix getMeasurementNoiseCov() {
170 return measurementNoiseCov;
171 }
172
173 /**
174 * Obtains estimated sample average.
175 *
176 * @return estimated sample average.
177 */
178 public double[] getSampleAverage() {
179 return sampleAverage;
180 }
181
182 /**
183 * Obtains number of samples used for estimation.
184 *
185 * @return number of samples used for estimation.
186 */
187 public long getSampleCount() {
188 return sampleCount;
189 }
190 }