1 /*
2 * Copyright (C) 2019 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.gnss;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20
21 /**
22 * Initializes the GNSS EKF Kalman filter state estimates and error covariance matrix.
23 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multi-sensor
24 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
25 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Initialize_GNSS_KF.m">
26 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Initialize_GNSS_KF.m
27 * </a>
28 */
29 public class GNSSKalmanInitializer {
30
31 /**
32 * Constructor.
33 * Prevents instantiation of utility class.
34 */
35 private GNSSKalmanInitializer() {
36 }
37
38 /**
39 * Initializes GNSS Kalman filter state.
40 *
41 * @param estimation initial GNSS estimation containing ECEF position and velocity,
42 * and estimated clock offset and drift.
43 * @param config Kalman filter configuration.
44 * @param result instance where resulting initialized Kalman filter state will be
45 * stored.
46 */
47 public static void initialize(final GNSSEstimation estimation,
48 final GNSSKalmanConfig config,
49 final GNSSKalmanState result) {
50 result.setEstimation(estimation);
51
52 final var initPosUnc = config.getInitialPositionUncertainty();
53 final var initVelUnc = config.getInitialVelocityUncertainty();
54 final var initClockOffsetUnc = config.getInitialClockOffsetUncertainty();
55 final var initClockDriftUnc = config.getInitialClockDriftUncertainty();
56
57 final var initPosUnc2 = initPosUnc * initPosUnc;
58 final var initVelUnc2 = initVelUnc * initVelUnc;
59 final var initClockOffsetUnc2 = initClockOffsetUnc * initClockOffsetUnc;
60 final var initClockDriftUnc2 = initClockDriftUnc * initClockDriftUnc;
61
62 Matrix covariance;
63 try {
64 covariance = new Matrix(GNSSEstimation.NUM_PARAMETERS, GNSSEstimation.NUM_PARAMETERS);
65
66 covariance.setElementAt(0, 0, initPosUnc2);
67 covariance.setElementAt(1, 1, initPosUnc2);
68 covariance.setElementAt(2, 2, initPosUnc2);
69
70 covariance.setElementAt(3, 3, initVelUnc2);
71 covariance.setElementAt(4, 4, initVelUnc2);
72 covariance.setElementAt(5, 5, initVelUnc2);
73
74 covariance.setElementAt(6, 6, initClockOffsetUnc2);
75 covariance.setElementAt(7, 7, initClockDriftUnc2);
76
77 result.setCovariance(covariance);
78
79 } catch (final WrongSizeException ignore) {
80 // never happens
81 }
82 }
83
84 /**
85 * Initializes GNSS Kalman filter state.
86 *
87 * @param estimation initial GNSS estimation containing ECEF position and velocity,
88 * and estimated clock offset and drift.
89 * @param config Kalman filter configuration.
90 * @return initialized Kalman filter state.
91 */
92 public static GNSSKalmanState initialize(final GNSSEstimation estimation, final GNSSKalmanConfig config) {
93 final var result = new GNSSKalmanState();
94 initialize(estimation, config, result);
95 return result;
96 }
97 }