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.inertial;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20
21 /**
22 * Initializes the loosely coupled INS/GNSS Kalman filter error covariance
23 * matrix.
24 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
25 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
26 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Initialize_LC_P_matrix.m">
27 * https://github.com/ymjdz/MATLAB-Codes/blob/master/Initialize_LC_P_matrix.m
28 * </a>
29 */
30 public class INSLooselyCoupledKalmanInitializer {
31
32 /**
33 * Number of parameters of the Kalman filter.
34 */
35 public static final int NUM_PARAMS = 15;
36
37 /**
38 * Constructor.
39 * Prevents instantiations of helper class.
40 */
41 private INSLooselyCoupledKalmanInitializer() {
42 }
43
44 /**
45 * Initializes INS/GNS loosely coupled Kalman filter error covariance matrix.
46 *
47 * @param config Kalman filter configuration.
48 * @param result instance where resulting initialized error covariance matrix
49 * will be stored. Matrix must be 15x15, otherwise it will be resized.
50 */
51 @SuppressWarnings("DuplicatedCode")
52 public static void initialize(final INSLooselyCoupledKalmanInitializerConfig config, final Matrix result) {
53 if (result.getRows() != NUM_PARAMS || result.getColumns() != NUM_PARAMS) {
54 try {
55 result.resize(NUM_PARAMS, NUM_PARAMS);
56 } catch (final WrongSizeException ignore) {
57 // never happens
58 }
59 }
60
61 final var initAttUnc = config.getInitialAttitudeUncertainty();
62 final var initVelUnc = config.getInitialVelocityUncertainty();
63 final var initPosUnc = config.getInitialPositionUncertainty();
64 final var initBaUnc = config.getInitialAccelerationBiasUncertainty();
65 final var initBgUnc = config.getInitialGyroscopeBiasUncertainty();
66
67 final var initAttUnc2 = initAttUnc * initAttUnc;
68 final var initVelUnc2 = initVelUnc * initVelUnc;
69 final var initPosUnc2 = initPosUnc * initPosUnc;
70 final var initBaUnc2 = initBaUnc * initBaUnc;
71 final var initBgUnc2 = initBgUnc * initBgUnc;
72
73 result.initialize(0.0);
74
75 for (var i = 0; i < 3; i++) {
76 result.setElementAt(i, i, initAttUnc2);
77 }
78 for (var i = 3; i < 6; i++) {
79 result.setElementAt(i, i, initVelUnc2);
80 }
81 for (var i = 6; i < 9; i++) {
82 result.setElementAt(i, i, initPosUnc2);
83 }
84 for (var i = 9; i < 12; i++) {
85 result.setElementAt(i, i, initBaUnc2);
86 }
87 for (var i = 12; i < 15; i++) {
88 result.setElementAt(i, i, initBgUnc2);
89 }
90 }
91
92 /**
93 * Initializes INS/GNSS loosely coupled Kalman filter error covariance matrix.
94 *
95 * @param config Kalman filter configuration.
96 * @return initialized error covariance matrix.
97 */
98 public static Matrix initialize(final INSLooselyCoupledKalmanInitializerConfig config) {
99 Matrix result = null;
100 try {
101 result = new Matrix(NUM_PARAMS, NUM_PARAMS);
102 initialize(config, result);
103 } catch (final WrongSizeException ignore) {
104 // never happens
105 }
106
107 return result;
108 }
109 }