CalcFocalPlaneAnglesMatrix.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_FOCAL_PLANE_ANGLES_MATRIX_HPP
#define GNC_NAVIGATION_MEASUREMENTS_CALC_FOCAL_PLANE_ANGLES_MATRIX_HPP
 
#include <cmath>
 
#include "gnc/navigation/measurements/CalcFocalPlaneAngles.hpp"
 
namespace clockwerk {
 
    /**
     * @brief Measurement class to calculate focal plane angle sensitivity matrix from tgt and observer state
     * 
     * This function calculates focal plane angle sensitivity matrix for a given target in
     * an image frame using the known position of the imager and target. The focal plane
     * angles are defined as follows:
     * alpha = atan([xtgt - xobs]/[ztgt - zobs])
     * beta = atan([ytgt - yobs]/[ztgt - zobs])
     * 
     * Where target and observer states are differenced to generate the target state in the
     * focal frame. The focal frame is defined as:
     * Centered in the center of the reference image
     * X is "up" in the image
     * Y is "right" in the image
     * Z is "into" the image completing the right hand frame
     * 
     * |----------------------|
     * |         x^           |
     * |          |-->y       |
     * |                      |
     * |----------------------|
     * 
     * The return is a flattened "H" matrix with values:
     * [dalpha/dx dalpha/dy dalpha/dz 0 0 0]
     * [dbeta/dx dbeta/dy dbeta/dz 0 0 0]
     */
    template <typename T>
    class CalcFocalPlaneAnglesMatrix : public clockwerk::Measurements<T, FOCAL_PLANE_MEAS_TARGET_STATES, FOCAL_PLANE_MEASUREMENTS*FOCAL_PLANE_MEAS_TARGET_STATES, FOCAL_PLANE_MEAS_OBSERVER_STATES> {
        public:
            /// @brief   Function to focal plane angles
            /// @param   time    The reference time (unused)
            /// @param   tar_state   The target reference state [x, y, z, vx, vy, vz]
            /// @param   obs_state   The observer reference state [x, y, z] representing camera position
            /// @param   out_measurements   Implicit return of [dalpha/dx dalpha/dy dalpha/dz 0 0 0, dbeta/dx dbeta/dy, dbeta/dz 0 0 0]
            int calculateMeasurements(T time, const std::array<T, FOCAL_PLANE_MEAS_TARGET_STATES> &tar_state, const std::array<T, FOCAL_PLANE_MEAS_OBSERVER_STATES> &obs_state,
                            std::array<T, FOCAL_PLANE_MEASUREMENTS*FOCAL_PLANE_MEAS_TARGET_STATES> &out_measurements);
        private:
 
    };
 
    template <typename T>
    int CalcFocalPlaneAnglesMatrix<T>::calculateMeasurements(T time, const std::array<T, FOCAL_PLANE_MEAS_TARGET_STATES> &tar_state, const std::array<T, FOCAL_PLANE_MEAS_OBSERVER_STATES> &obs_state,
                            std::array<T, FOCAL_PLANE_MEASUREMENTS*FOCAL_PLANE_MEAS_TARGET_STATES> &out_measurements) {
        // Pre-calculate parameters 
        T focal_x = tar_state[0] - obs_state[0];
        T focal_y = tar_state[1] - obs_state[1];
        T focal_z = tar_state[2] - obs_state[2];
        T one_over_xsquared_zsquared, one_over_ysquared_zsquared;
        int err = safeDivide(1.0, focal_x*focal_x + focal_z*focal_z, one_over_xsquared_zsquared);
        if(err) {return err;}
        err = safeDivide(1.0, focal_y*focal_y + focal_z*focal_z, one_over_ysquared_zsquared);
        if(err) {return err;}
 
        out_measurements[0] = focal_z*one_over_xsquared_zsquared;
        out_measurements[1] = 0.0;
        out_measurements[2] = -focal_x*one_over_xsquared_zsquared;
        out_measurements[3] = 0.0;
        out_measurements[4] = 0.0;
        out_measurements[5] = 0.0;
        out_measurements[6] = 0.0;
        out_measurements[7] = focal_z*one_over_ysquared_zsquared;
        out_measurements[8] = -focal_y*one_over_ysquared_zsquared;
        out_measurements[9] = 0.0;
        out_measurements[10] = 0.0;
        out_measurements[11] = 0.0;
 
        return NO_ERROR;
    };
 
}
 
#endif