PointingVectorMeasurements.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_POINTING_VECTOR_MEASUREMENTS_H
#define GNCUTILS_EKF_POINTING_VECTOR_MEASUREMENTS_H
 
#include "gncutils/EKF/Measurements.hpp"
#include "dynamics/Quaternion.h"
 
namespace warpos {
 
    // ***** Define the number of measurement vector elements, observer vector elements, and measurement noise elements ***** //
    // Pointing vector (e.g. magnetometer, sun sensor) expressed in body frame coordinates
    const uint32 POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS(3);
    // Here observer state acts as a parameter inputs and must include: Expected pointing vector experienced by the
    // sensor expressed in body frame coordinates.
    const uint32 POINTING_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS(3);
    // Every element of the measurement has an associated Gaussian noise.
    const uint32 POINTING_MEASUREMENT_MEASUREMENT_NOISE_ELEMENTS(3);

    template<uint32 N>
    class PointingVectorMeasurements : public Measurements<N, POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS, POINTING_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS>{
    public:
        PointingVectorMeasurements(uint32 state_quat_index, uint32 observer_pointing_index = 0, uint32 observer_quat_index = 3, uint32 meas_pointing_index = 0) :
            _quat_idx(state_quat_index),
            _obs_pointing_idx(observer_pointing_index),
            _obs_quat_idx(observer_quat_index),
            _meas_pointing_idx(meas_pointing_index) {};

        int16 calculateMeasurements(floating_point time, 
                                    const std::array<floating_point, N> &tar_state, 
                                    const std::array<floating_point, POINTING_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> &obs_state,
                                    floating_point expected_measurement[POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS]) override;

        int16 calculateMeasurementsMatrix(floating_point time,
                                          const std::array<floating_point, N> &tar_state,
                                          const std::array<floating_point, POINTING_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> &obs_state,
                                          floating_point measurement_Jacobian[N*POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS]) override;
 
    private:
        uint32 _quat_idx;
        uint32 _obs_pointing_idx;
        uint32 _obs_quat_idx;
        uint32 _meas_pointing_idx;

        clockwerk::Quaternion _quat_body_ref;
        clockwerk::CartesianVector<3> _mag__ref;
        clockwerk::Quaternion _quat_mag_body;

        clockwerk::CartesianVector<3> _dummy3;
    };
 

    template<uint32 N>
    int16 PointingVectorMeasurements<N>::calculateMeasurements(floating_point time, 
                                    const std::array<floating_point, N> &tar_state, 
                                    const std::array<floating_point, POINTING_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> &obs_state,
                                    floating_point expected_measurement[POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS]) {
        // Gather the attitude quaternion
        _quat_body_ref.setFromArray(&tar_state[_quat_idx]);
        // Gather the mag field vector from params
        _mag__ref.setFromArray(&obs_state[_obs_pointing_idx]);
        // Rotate the expected local mag field from reference frame to body frame
        _dummy3 = _quat_body_ref*_mag__ref;
        // Output the result
        _dummy3.getAsArray(&expected_measurement[_meas_pointing_idx]);
 
        return NO_ERROR;
    }

    template<uint32 N>
    int16 PointingVectorMeasurements<N>::calculateMeasurementsMatrix(floating_point time,
                                    const std::array<floating_point, N> &tar_state,
                                    const std::array<floating_point, POINTING_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS> &obs_state,
                                    floating_point measurement_Jacobian[N*POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS]) {
        // Make sure quaternion index doesn't overrun the array
        if (_quat_idx + 4 > N) {return ERROR_DIMENSIONS;}
 
        // Gather the attitude quaternion
        _quat_body_ref.setFromArray(&tar_state[_quat_idx]);
 
        // Gather the mag field vector from params
        _mag__ref.setFromArray(&obs_state[_obs_pointing_idx]);
 
        // Compute the nonzero Jacobian term
        clockwerk::Matrix<3, 4> dhdq = _quat_body_ref.dRotatedVec_dQuat(_mag__ref);
 
        // Make matrix of all zeros and edit the correct terms
        for(uint32 i = 0; i < N*POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS; i++) {
            measurement_Jacobian[i] = 0.0;
        }
        for (uint32 i = 0; i < 3; ++i) {
            for (uint32 j = 0; j < 4; ++j) {
                measurement_Jacobian[i*N + j+_quat_idx] = dhdq.get(i, j);
            }
        }
        return NO_ERROR;
    }
}
 
#endif