GPSMeasurements.hpp

Go to the documentation of this file.

/******************************************************************************
* Copyright (c) ATTX LLC 2024. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, LLC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, LLC.
******************************************************************************/
 
#ifndef GNCUTILS_EKF_GPS_MEASUREMENTS_HPP
#define GNCUTILS_EKF_GPS_MEASUREMENTS_HPP
 
#include "gncutils/EKF/Measurements.hpp"
#include "dynamics/Quaternion.h"
#include "constants/mathconstants.h"
 
namespace warpos {
 
    // ***** Define the number of measurement vector elements, observer vector elements, and measurement noise elements ***** //
    // Position as defined in the state vector
    const uint32 GPS_MEASUREMENT_VECTOR_ELEMENTS(3);
    // Here observer state acts as a parameter inputs and must include: The attitude of the frame the state is resolved in
    // with respect to the attitude of the frame the GPS sensor reports in, provided as a unit quaternion.
    const uint32 GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS(4);
    // Every element of the measurement has an associated Gaussian noise.
    const uint32 GPS_MEASUREMENT_MEASUREMENT_NOISE_ELEMENTS(3);

    template<uint32 N>
    class GPSMeasurements : public Measurements<N, GPS_MEASUREMENT_VECTOR_ELEMENTS, GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS>{
    public:
        GPSMeasurements(uint32 state_pos_index, uint32 observer_quat_index = 0, uint32 meas_pos_index = 0) :
            _pos_idx(state_pos_index),
            _obs_quat_idx(observer_quat_index),
            _meas_pos_idx(meas_pos_index) {};
        
        int16 calculateMeasurements(floating_point time, 
                                    const std::array<floating_point, N> &tar_state, 
                                    const std::array<floating_point, GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> &obs_state,
                                    floating_point expected_measurement[GPS_MEASUREMENT_VECTOR_ELEMENTS]) override;

        int16 calculateMeasurementsMatrix(floating_point time,
                                          const std::array<floating_point, N> &tar_state,
                                          const std::array<floating_point, GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> &obs_state,
                                          floating_point measurement_Jacobian[N*GPS_MEASUREMENT_VECTOR_ELEMENTS]) override;
 
    private:
        uint32 _pos_idx;
        uint32 _obs_quat_idx;
        uint32 _meas_pos_idx;
    };
 

    template<uint32 N>
    int16 GPSMeasurements<N>::calculateMeasurements(floating_point time, 
                                    const std::array<floating_point, N> &tar_state, 
                                    const std::array<floating_point, GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> &obs_state,
                                    floating_point expected_measurement[GPS_MEASUREMENT_VECTOR_ELEMENTS]) {
        // Measurement is the same as the position part of the state
        expected_measurement[_meas_pos_idx] = tar_state[_pos_idx];
        expected_measurement[_meas_pos_idx+1] = tar_state[_pos_idx+1];
        expected_measurement[_meas_pos_idx+2] = tar_state[_pos_idx+2];
 
        return NO_ERROR;
    }

    template<uint32 N>
    int16 GPSMeasurements<N>::calculateMeasurementsMatrix(floating_point time,
                                    const std::array<floating_point, N> &tar_state,
                                    const std::array<floating_point, GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> &obs_state,
                                    floating_point measurement_Jacobian[N*GPS_MEASUREMENT_VECTOR_ELEMENTS]) {
        // Make sure quaternion index doesn't overrun the array
        if (_pos_idx + 3 > N) {return ERROR_DIMENSIONS;}
 
        // Make matrix of all zeros and edit the correct terms
        for(uint32 i = 0; i < N*GPS_MEASUREMENT_VECTOR_ELEMENTS; i++) {
            measurement_Jacobian[i] = 0.0;
        }
        for (uint32 i = 0; i < 3; ++i) {
            for (uint32 j = 0; j < 3; ++j) {
                measurement_Jacobian[i*N + j+_pos_idx] = IDENTITY_3_3.get(i, j);
            }
        }
        return NO_ERROR;
    }
 
}
 
#endif