INSTightlyCoupledKalmanInitializer.java

/*
 * Copyright (C) 2019 Alberto Irurueta Carro (alberto@irurueta.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package com.irurueta.navigation.inertial;

import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.WrongSizeException;

/**
 * Initializes the tightly coupled INS/GNSS extended Kalman filter error covariance
 * matrix.
 * This implementation is based on the equations defined in "Principles of GNSS, Inertial, and Multisensor
 * Integrated Navigation Systems, Second Edition" and on the companion software available at:
 * <a href="https://github.com/ymjdz/MATLAB-Codes/blob/master/Initialize_TC_P_matrix.m">
 *     https://github.com/ymjdz/MATLAB-Codes/blob/master/Initialize_TC_P_matrix.m
 * </a>
 */
public class INSTightlyCoupledKalmanInitializer {

    /**
     * Number of parameters of the Kalman filter.
     */
    public static final int NUM_PARAMS = 17;

    /**
     * Constructor.
     * Prevents instantiation of helper class.
     */
    private INSTightlyCoupledKalmanInitializer() {
    }

    /**
     * Initializes INS/GNS tightly coupled Kalman filter error covariance matrix.
     *
     * @param config Kalman filter configuration.
     * @param result instance where resulting initialized error covariance matrix
     *               will be stored. Matrix must be 17x17, otherwise it will be resized.
     */
    @SuppressWarnings("DuplicatedCode")
    public static void initialize(final INSTightlyCoupledKalmanInitializerConfig config, final Matrix result) {
        if (result.getRows() != NUM_PARAMS || result.getColumns() != NUM_PARAMS) {
            try {
                result.resize(NUM_PARAMS, NUM_PARAMS);
            } catch (final WrongSizeException ignore) {
                // never happens
            }
        }

        final var initAttUnc = config.getInitialAttitudeUncertainty();
        final var initVelUnc = config.getInitialVelocityUncertainty();
        final var initPosUnc = config.getInitialPositionUncertainty();
        final var initBaUnc = config.getInitialAccelerationBiasUncertainty();
        final var initBgUnc = config.getInitialGyroscopeBiasUncertainty();
        final var initClockOffset = config.getInitialClockOffsetUncertainty();
        final var initClockDrift = config.getInitialClockDriftUncertainty();

        final var initAttUnc2 = initAttUnc * initAttUnc;
        final var initVelUnc2 = initVelUnc * initVelUnc;
        final var initPosUnc2 = initPosUnc * initPosUnc;
        final var initBaUnc2 = initBaUnc * initBaUnc;
        final var initBgUnc2 = initBgUnc * initBgUnc;
        final var initClockOffset2 = initClockOffset * initClockOffset;
        final var initClockDrift2 = initClockDrift * initClockDrift;

        result.initialize(0.0);

        for (var i = 0; i < 3; i++) {
            result.setElementAt(i, i, initAttUnc2);
        }
        for (var i = 3; i < 6; i++) {
            result.setElementAt(i, i, initVelUnc2);
        }
        for (var i = 6; i < 9; i++) {
            result.setElementAt(i, i, initPosUnc2);
        }
        for (var i = 9; i < 12; i++) {
            result.setElementAt(i, i, initBaUnc2);
        }
        for (var i = 12; i < 15; i++) {
            result.setElementAt(i, i, initBgUnc2);
        }
        result.setElementAt(15, 15, initClockOffset2);
        result.setElementAt(16, 16, initClockDrift2);
    }

    /**
     * Initializes INS/GNS tightly coupled Kalman filter error covariance matrix.
     *
     * @param config Kalman filter configuration.
     * @return initialized error covariance matrix.
     */
    public static Matrix initialize(final INSTightlyCoupledKalmanInitializerConfig config) {
        Matrix result = null;
        try {
            result = new Matrix(NUM_PARAMS, NUM_PARAMS);
            initialize(config, result);
        } catch (final WrongSizeException ignore) {
            // never happens
        }

        return result;
    }
}