WsfTRIMSIM_Processor.hpp

Go to the documentation of this file.

// ****************************************************************************
// CUI//REL TO USA ONLY
//
// The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
//
// Copyright 2016 Infoscitex, a DCS Company. All rights reserved.
//
// The use, dissemination or disclosure of data in this file is subject to
// limitation or restriction. See accompanying README and LICENSE for details.
// ****************************************************************************
 
#ifndef WSFTRIMSIM_PROCESSOR_HPP
#define WSFTRIMSIM_PROCESSOR_HPP
 
#include "wsf_nx_export.h"
 
#include <memory>
 
#include "UtMatrix.hpp"
#include "WsfSensorComponent.hpp"
#include "WsfSensorErrorModel.hpp"
#include "WsfSensorProcessor.hpp"
class WsfTRIMSIM_SensorComponent;

class WSF_NX_EXPORT WsfTRIMSIM_Processor : public WsfSensorProcessor
{
public:
   static constexpr unsigned int cDEFAULT_MINIMUM_DETECTIONS = 4U;

   struct Detection
   {
      WsfSensor* mSensorPtr;
      size_t     mTargetPlatformIndex;
      double     mSensorLocWCS[3];
      double     mSensorVelNED[3];
      double     mSensorVelWCS[3];
      double     mTimeOfDetection;
      double     mTrueRange;
      double     mTrueUnitVecNED[3];
   };
   using Detections = std::vector<Detection>;
 
   explicit WsfTRIMSIM_Processor(WsfScenario& aScenario);
   ~WsfTRIMSIM_Processor() override = default;


   WsfProcessor* Clone() const override;
   bool          ProcessInput(UtInput& aInput) override;


   void TurnOn(double aSimTime) override;
 
   bool AttemptToDetect(double aSimTime, WsfSensor* aSensorPtr, WsfPlatform* aTargetPtr, WsfSensorResult& aResult);
 
   bool ComputeMeasurementErrors(WsfSensor& aSensor);


   const Detections& GetDetections() const { return mDetectionList; }
   size_t            GetDetectionCount() const { return mDetectionList.size(); }
 
   static void PrintMatrix(const std::string& aTitle, UtMatrixd& aMatrix);
 
private:
   WsfTRIMSIM_Processor(const WsfTRIMSIM_Processor& aSrc);
   WsfTRIMSIM_Processor& operator=(const WsfTRIMSIM_Processor&) = delete;



   void CheckSensors();
   bool SensorAdded(WsfSensor* aSensorPtr) override;
   bool SensorRemoved(WsfSensor* aSensorPtr) override;


   void AddDetection(double aSimTime, WsfSensor* aSensorPtr, size_t aTargetPlatformIndex, WsfSensorResult& aResult);
   void ClearDetections() { mDetectionList.clear(); }
 
   void PrintTDOAInfo(WsfSensor* aSensorPtr);

   unsigned int mMinimumDetections{cDEFAULT_MINIMUM_DETECTIONS};

   std::vector<Detection> mDetectionList;

   std::vector<WsfSensor*> mSensorPtrs;
};
 
#include "WsfNonExportableComponentRoles.hpp"

class WSF_NX_EXPORT WsfTRIMSIM_SensorComponent : public WsfSensorComponent
{
public:
   static void                        RegisterComponentFactory(WsfScenario& aScenario);
   static WsfTRIMSIM_SensorComponent* Find(const WsfSensor& aSensor);
   static WsfTRIMSIM_SensorComponent* FindOrCreate(WsfSensor& aSensor);
 
   WsfTRIMSIM_SensorComponent();
   WsfTRIMSIM_SensorComponent(const WsfTRIMSIM_SensorComponent& aSrc);
   ~WsfTRIMSIM_SensorComponent() override = default;
   WsfTRIMSIM_SensorComponent& operator=(const WsfTRIMSIM_SensorComponent&) = delete;


   WsfComponent* CloneComponent() const override;
   WsfStringId   GetComponentName() const override;
   const int*    GetComponentRoles() const override;
   void*         QueryInterface(int aRole) override;


   void PostAttemptToDetect(double aSimTime, WsfPlatform* aTargetPtr, WsfSensorResult& aResult) override;
 
   void UpdateTrack(double aSimTime, WsfTrack* aTrackPtr, WsfPlatform* aTargetPtr, WsfSensorResult& aResult) override;
 
   bool GetMeasurementIsValid() const { return mMeasurementIsValid; }
   void SetMeasurementIsValid(bool aIsValid) { mMeasurementIsValid = aIsValid; }
 
   UtMatrixd& GetMeasurement() { return mMeasurement; }
   void       SetMeasurement(const UtMatrixd& aMeasurement) { mMeasurement = aMeasurement; }
 
   UtMatrixd& GetMeasurementErrorCovariance() { return mMeasurementErrorCovariance; }
   void       SetMeasurementErrorCovariance(const UtMatrixd& aMeasurementErrorCovariance)
   {
      mMeasurementErrorCovariance = aMeasurementErrorCovariance;
   }
 
   UtMatrixd& GetMeasurementTransition() { return mMeasurementTransition; }
   void       SetMeasurementTransition(const UtMatrixd& aMeasurementTransition)
   {
      mMeasurementTransition = aMeasurementTransition;
   }
 
   void CorrectForAltitude(double       aTargetLocWCS[3], // in / out
                           WsfPlatform* aTargetPtr,
                           double       aDefaultAltitude = 0.0);
 
   void SetProcessor(WsfTRIMSIM_Processor* aTRIMSIM_ProcessorPtr) { mTRIMSIM_ProcessorPtr = aTRIMSIM_ProcessorPtr; }
 
private:
   void ConvertMatrix(UtMatrixd& aMatrix, WsfTrack* aTrackPtr);
 
   void ApplyMeasurementErrors(double aSimTime, WsfTrack* aTrackPtr, WsfPlatform* aTargetPtr, WsfSensorResult& aResult);


   UtMatrixd mMeasurement;                
   UtMatrixd mMeasurementErrorCovariance; 
   UtMatrixd mMeasurementTransition;      
   bool      mMeasurementIsValid;
 
   WsfTRIMSIM_Processor* mTRIMSIM_ProcessorPtr;
};
 
WSF_DECLARE_COMPONENT_ROLE_TYPE(WsfTRIMSIM_SensorComponent, cWSF_COMPONENT_TRIMSIM_SENSOR)
 

class WSF_NX_EXPORT WsfTRIMSIM_ErrorModel : public wsf::SensorErrorModelBase
{
public:
   WsfTRIMSIM_ErrorModel();
 
   static std::unique_ptr<wsf::SensorErrorModelBase> ObjectFactory(const std::string& aTypeName);
 
   WsfTRIMSIM_ErrorModel* Clone() const override;
   bool                   ProcessInput(UtInput& aInput) override;
   bool                   Initialize(WsfSensorMode* aSensorModePtr) override;
 
   void ApplyMeasurementErrors(WsfSensorResult& aResult) override
   {
      // This class does not apply errors; it is done in WsfTRIMSIM_Processor::ApplyMeasurementErrors.
   }
   bool IsNullModel() const override { return true; }


   bool                       ComputeErrors() const { return mComputeErrors; }
   const std::vector<double>& GetErrors() const { return mErrors; }
   double                     GetGroundTargetAltitudeError() const { return mGroundTargetAltitudeError; }
 
   static const unsigned int cTDOA_ERROR_SIZE;
   static const unsigned int cTDOA_STATE_SIZE;
 
protected:
   WsfTRIMSIM_ErrorModel(const WsfTRIMSIM_ErrorModel& aSrc) = default;
   WsfTRIMSIM_ErrorModel& operator=(const WsfTRIMSIM_ErrorModel&) = delete;
 
private:
   // double  mTDOA_Errors[7];
   // north position error sigma (m)
   // east position error sigma (m)
   // down position error sigma (m)
   // reference time error (s)
   // inter-system time error (s)
   // sensor timing error (s)
   // atmospheric refraction error
 
   bool                mComputeErrors;             
   std::vector<double> mErrors;                    
   double              mGroundTargetAltitudeError; 
};
 
#endif