CalcRangeRangeRateMatrix.hpp

// /******************************************************************************
// * 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 GNC_NAVIGATION_MEASUREMENTS_CALC_RANGE_RANGERATE_MATRIX_HPP
#define GNC_NAVIGATION_MEASUREMENTS_CALC_RANGE_RANGERATE_MATRIX_HPP
 
#include "utils/Measurements.hpp"
 
#define R_RR_MATRIX_H_STATES 12
#define R_RR_MATRIX_TGT_STATES 6
#define R_RR_MATRIX_OBS_STATES 6
#define R_RR_MATRIX_MEAS 2
 
namespace clockwerk {
 
    template <typename T>
    class CalcRangeRangeRateMatrix : public Measurements<T, R_RR_MATRIX_TGT_STATES, R_RR_MATRIX_H_STATES, R_RR_MATRIX_OBS_STATES> {
    public:
        /// @brief   Function to calculate Measurement Sensitivity Matrix
        /// @param   time    The reference time
        /// @param   tar_state   The target reference state [x, y, z, vx, vy, vz]
        /// @param   obs_state   The observer reference state ["]
        int calculateMeasurements(T time, const std::array<T, R_RR_MATRIX_TGT_STATES> &tar_state, const std::array<T, R_RR_MATRIX_OBS_STATES> &obs_state,
                            std::array<T, R_RR_MATRIX_H_STATES> &out_measurements);
 
    private:
 
    };
 
    template <typename T>
    int CalcRangeRangeRateMatrix<T>::calculateMeasurements(T time, const std::array<T, R_RR_MATRIX_TGT_STATES> &tar_state, const std::array<T, R_RR_MATRIX_OBS_STATES> &obs_state,
                                                           std::array<T, R_RR_MATRIX_H_STATES> &out_measurements) {
        // Now pre-calculate some simple parameters
        // Calculate range and range^3
        double xdiff = tar_state[0] - obs_state[0];
        double ydiff = tar_state[1] - obs_state[1];
        double zdiff = tar_state[2] - obs_state[2];
        double xdotdiff = tar_state[3] - obs_state[3];
        double ydotdiff = tar_state[4] - obs_state[4];
        double zdotdiff = tar_state[5] - obs_state[5];
        double range = std::sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff);
        double range3 = range*range*range;
        double one_over_range, one_over_range3;
        int err = clockwerk::safeDivide(1.0, range, one_over_range);
        if(err) {return err;}
        err = clockwerk::safeDivide(1.0, range3, one_over_range3);
        if(err) {return err;}
 
        // Calculate each element of our matrix:
        out_measurements[0] = xdiff*one_over_range;
        out_measurements[1] = ydiff*one_over_range;
        out_measurements[2] = zdiff*one_over_range;
 
        out_measurements[3] = 0.0;
        out_measurements[4] = 0.0;
        out_measurements[5] = 0.0;
 
        out_measurements[6] = -((xdiff*(xdotdiff*xdiff + ydotdiff*ydiff+zdotdiff*zdiff))*one_over_range3 - xdotdiff*one_over_range);
        out_measurements[7] = -((ydiff*(xdotdiff*xdiff + ydotdiff*ydiff+zdotdiff*zdiff))*one_over_range3 - ydotdiff*one_over_range);
        out_measurements[8] = -((zdiff*(xdotdiff*xdiff + ydotdiff*ydiff+zdotdiff*zdiff))*one_over_range3 - zdotdiff*one_over_range);
 
        out_measurements[9] = out_measurements[0];
        out_measurements[10] = out_measurements[1];
        out_measurements[11] = out_measurements[2];
 
        return NO_ERROR;
    }
 
}
 
#endif