1 /*
2 * Copyright (C) 2020 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.navigation.inertial.calibration;
17
18 import com.irurueta.navigation.LockedException;
19 import com.irurueta.units.Time;
20 import com.irurueta.units.TimeConverter;
21 import com.irurueta.units.TimeUnit;
22
23
24 /**
25 * Estimates average time interval between processed samples.
26 */
27 public class TimeIntervalEstimator {
28
29 /**
30 * Default total samples to be processed.
31 */
32 public static final int DEFAULT_TOTAL_SAMPLES = 100000;
33
34 /**
35 * Total samples to be processed to finish estimation.
36 */
37 private int totalSamples = DEFAULT_TOTAL_SAMPLES;
38
39 /**
40 * Listener to handle events raised by this estimator.
41 */
42 private TimeIntervalEstimatorListener listener;
43
44 /**
45 * Last provided timestamp expressed in seconds (s).
46 */
47 private Double lastTimestamp;
48
49 /**
50 * Estimated average time interval between body kinematics samples expressed in
51 * seconds (s).
52 */
53 private double averageTimeInterval;
54
55 /**
56 * Estimated variance of time interval between body kinematics samples expressed
57 * in squared seconds (s^2).
58 */
59 private double timeIntervalVariance;
60
61 /**
62 * Number of processed timestamp samples.
63 */
64 private int numberOfProcessedSamples;
65
66 /**
67 * Number of processed timestamp samples plus one.
68 */
69 private int numberOfProcessedSamplesPlusOne = 1;
70
71 /**
72 * Indicates that estimator is running.
73 */
74 private boolean running;
75
76 /**
77 * Constructor.
78 */
79 public TimeIntervalEstimator() {
80 }
81
82 /**
83 * Constructor.
84 *
85 * @param listener listener to handle events raised by this estimator.
86 */
87 public TimeIntervalEstimator(final TimeIntervalEstimatorListener listener) {
88 this.listener = listener;
89 }
90
91 /**
92 * Constructor.
93 *
94 * @param totalSamples total samples to be processed to finish estimation.
95 * @throws IllegalArgumentException if provided total samples is zero or negative.
96 */
97 public TimeIntervalEstimator(final int totalSamples) {
98 if (totalSamples <= 0) {
99 throw new IllegalArgumentException();
100 }
101
102 this.totalSamples = totalSamples;
103 }
104
105 /**
106 * Constructor.
107 *
108 * @param totalSamples total samples to be processed to finish estimation.
109 * @param listener listener to handle events raised by this estimator.
110 * @throws IllegalArgumentException if provided total samples is zero or negative.
111 */
112 public TimeIntervalEstimator(final int totalSamples, final TimeIntervalEstimatorListener listener) {
113 this(totalSamples);
114 this.listener = listener;
115 }
116
117 /**
118 * Copy constructor.
119 *
120 * @param input instance to get a copy from.
121 */
122 public TimeIntervalEstimator(final TimeIntervalEstimator input) {
123 copyFrom(input);
124 }
125
126 /**
127 * Gets total samples to be processed to finish estimation.
128 *
129 * @return total samples to be processed to finish estimation.
130 */
131 public int getTotalSamples() {
132 return totalSamples;
133 }
134
135 /**
136 * Sets total samples to be processed to finish estimation.
137 *
138 * @param totalSamples total samples to be processed to finish estimation.
139 * @throws LockedException if estimator is currently running.
140 */
141 public void setTotalSamples(final int totalSamples) throws LockedException {
142 if (running) {
143 throw new LockedException();
144 }
145
146 if (totalSamples <= 0) {
147 throw new IllegalArgumentException();
148 }
149
150 this.totalSamples = totalSamples;
151 }
152
153 /**
154 * Gets listener to handle events raised by this estimator.
155 *
156 * @return listener to handle events raised by this estimator.
157 */
158 public TimeIntervalEstimatorListener getListener() {
159 return listener;
160 }
161
162 /**
163 * Sets listener to handle events raised by this estimator.
164 *
165 * @param listener listener to handle events raised by this estimator.
166 * @throws LockedException if this estimator is running.
167 */
168 public void setListener(final TimeIntervalEstimatorListener listener) throws LockedException {
169 if (running) {
170 throw new LockedException();
171 }
172
173 this.listener = listener;
174 }
175
176 /**
177 * Gets last provided timestamp expressed in seconds or null if none has been
178 * provided yet.
179 *
180 * @return last provided timestamp or null.
181 */
182 public Double getLastTimestamp() {
183 return lastTimestamp;
184 }
185
186 /**
187 * Gets last provided timestamp or null if none has been provided yet.
188 *
189 * @return last provided timestamp or null.
190 */
191 public Time getLastTimestampAsTime() {
192 return lastTimestamp != null ? new Time(lastTimestamp, TimeUnit.SECOND) : null;
193 }
194
195 /**
196 * Gets last provided timestamp.
197 *
198 * @param result instance where last provided timestamp will be stored.
199 * @return true if last provided timestamp was available, false otherwise.
200 */
201 public boolean getLastTimestampAsTime(final Time result) {
202 if (lastTimestamp != null) {
203 result.setValue(lastTimestamp);
204 result.setUnit(TimeUnit.SECOND);
205 return true;
206 } else {
207 return false;
208 }
209 }
210
211 /**
212 * Gets estimated average time interval between body kinematics samples expressed in
213 * seconds (s).
214 * Calling this method before the estimator is finished will return a provisional
215 * value containing current estimation.
216 *
217 * @return estimated average time interval.
218 */
219 public double getAverageTimeInterval() {
220 return averageTimeInterval;
221 }
222
223 /**
224 * Gets estimated average time interval between body kinematics samples.
225 * Calling this method before the estimator is finished will return a provisional
226 * value containing current estimation.
227 *
228 * @return estimate average time interval.
229 */
230 public Time getAverageTimeIntervalAsTime() {
231 return new Time(averageTimeInterval, TimeUnit.SECOND);
232 }
233
234 /**
235 * Gets estimated average time interval between body kinematics samples.
236 * Calling this method before the estimator is finished will return a provisional
237 * value containing current estimation.
238 *
239 * @param result instance where estimated average time interval will be stored.
240 */
241 public void getAverageTimeIntervalAsTime(final Time result) {
242 result.setValue(averageTimeInterval);
243 result.setUnit(TimeUnit.SECOND);
244 }
245
246 /**
247 * Gets estimated variance of time interval between body kinematics samples
248 * expressed in squared seconds (s^2).
249 * Calling this method before the estimator is finished will return a provisional
250 * value containing current estimation.
251 *
252 * @return estimated variance of time interval between body kinematics samples.
253 */
254 public double getTimeIntervalVariance() {
255 return timeIntervalVariance;
256 }
257
258 /**
259 * Gets estimate standard deviation of time interval between body kinematics
260 * samples expressed in seconds (s).
261 * Calling this method before the estimator is finished will return a provisional
262 * value containing current estimation.
263 *
264 * @return estimated standard deviation of time interval between body kinematics
265 * samples.
266 */
267 public double getTimeIntervalStandardDeviation() {
268 return Math.sqrt(timeIntervalVariance);
269 }
270
271 /**
272 * Gets estimate standard deviation of time interval between body kinematics
273 * samples.
274 * Calling this method before the estimator is finished will return a provisional
275 * value containing current estimation.
276 *
277 * @return estimated standard deviation of time interval between body kinematics
278 * samples.
279 */
280 public Time getTimeIntervalStandardDeviationAsTime() {
281 return new Time(getTimeIntervalStandardDeviation(), TimeUnit.SECOND);
282 }
283
284 /**
285 * Gets estimate standard deviation of time interval between body kinematics
286 * samples.
287 * Calling this method before the estimator is finished will return a provisional
288 * value containing current estimation.
289 *
290 * @param result instance where estimated standard deviation of time interval
291 * between body kinematics samples will be stored.
292 */
293 public void getTimeIntervalStandardDeviationAsTime(final Time result) {
294 result.setValue(getTimeIntervalStandardDeviation());
295 result.setUnit(TimeUnit.SECOND);
296 }
297
298 /**
299 * Gets number of samples that have been processed so far.
300 *
301 * @return number of samples that have been processed so far.
302 */
303 public int getNumberOfProcessedSamples() {
304 return numberOfProcessedSamples;
305 }
306
307 /**
308 * Indicates whether estimator is currently running or not.
309 *
310 * @return true if estimator is running, false otherwise.
311 */
312 public boolean isRunning() {
313 return running;
314 }
315
316 /**
317 * Indicates whether estimator has finished the estimation.
318 *
319 * @return true if estimator has finished, false otherwise.
320 */
321 public boolean isFinished() {
322 return numberOfProcessedSamples == totalSamples;
323 }
324
325 /**
326 * Adds a timestamp value to current estimation.
327 * If estimator is already finished, provided timestamp will be ignored.
328 *
329 * @param timestamp timestamp since epoch time.
330 * @return true if provided timestamp has been processed, false if it has been
331 * ignored.
332 * @throws LockedException if estimator is currently running.
333 */
334 public boolean addTimestamp(final Time timestamp) throws LockedException {
335 return addTimestamp(TimeConverter.convert(timestamp.getValue().doubleValue(), timestamp.getUnit(),
336 TimeUnit.SECOND));
337 }
338
339 /**
340 * Adds a timestamp value to current estimation.
341 * If estimator is already finished, provided timestamp will be ignored.
342 *
343 * @param timestamp timestamp since epoch time expressed in seconds (s).
344 * @return true if provided timestamp has been processed, false if it has been
345 * ignored.
346 * @throws LockedException if estimator is currently running.
347 */
348 public boolean addTimestamp(final double timestamp) throws LockedException {
349 if (running) {
350 throw new LockedException();
351 }
352
353 if (isFinished()) {
354 return false;
355 }
356
357 running = true;
358
359 if (lastTimestamp == null && listener != null) {
360 listener.onStart(this);
361 }
362
363 if (lastTimestamp != null) {
364 final var timeInterval = timestamp - lastTimestamp;
365
366 averageTimeInterval = averageTimeInterval * numberOfProcessedSamples / numberOfProcessedSamplesPlusOne
367 + timeInterval / numberOfProcessedSamplesPlusOne;
368
369 final var diff = timeInterval - averageTimeInterval;
370 final var diff2 = diff * diff;
371 timeIntervalVariance = timeIntervalVariance * numberOfProcessedSamples / numberOfProcessedSamplesPlusOne
372 + diff2 / numberOfProcessedSamplesPlusOne;
373 }
374
375 lastTimestamp = timestamp;
376
377 numberOfProcessedSamples++;
378 numberOfProcessedSamplesPlusOne++;
379
380 if (listener != null) {
381 listener.onTimestampAdded(this);
382 }
383
384 running = false;
385
386 if (isFinished() && listener != null) {
387 listener.onFinish(this);
388 }
389
390 return true;
391 }
392
393 /**
394 * Resets current estimator.
395 *
396 * @return true if estimator was successfully reset, false if no reset was needed.
397 * @throws LockedException if estimator is currently running.
398 */
399 @SuppressWarnings("DuplicatedCode")
400 public boolean reset() throws LockedException {
401 if (running) {
402 throw new LockedException();
403 }
404
405 if (numberOfProcessedSamples == 0) {
406 return false;
407 }
408
409 running = true;
410 lastTimestamp = null;
411 averageTimeInterval = 0.0;
412 timeIntervalVariance = 0.0;
413 numberOfProcessedSamples = 0;
414 numberOfProcessedSamplesPlusOne = 1;
415
416 if (listener != null) {
417 listener.onReset(this);
418 }
419
420 running = false;
421
422 return true;
423 }
424
425 /**
426 * Copies instance from provided one into this one.
427 *
428 * @param input input instance to be copied.
429 */
430 public void copyFrom(final TimeIntervalEstimator input) {
431 totalSamples = input.totalSamples;
432 listener = input.listener;
433 lastTimestamp = input.lastTimestamp;
434 averageTimeInterval = input.averageTimeInterval;
435 timeIntervalVariance = input.timeIntervalVariance;
436 numberOfProcessedSamples = input.numberOfProcessedSamples;
437 numberOfProcessedSamplesPlusOne = input.numberOfProcessedSamplesPlusOne;
438 running = input.running;
439 }
440
441 /**
442 * Copies current instance into provided instance.
443 *
444 * @param output output instance to copy to.
445 */
446 public void copyTo(final TimeIntervalEstimator output) {
447 output.copyFrom(this);
448 }
449 }