/* This file is part of MAUS: http://micewww.pp.rl.ac.uk:8080/projects/maus * * MAUS is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * MAUS is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with MAUS. If not, see . * */ #ifndef KALMAN_TRACK_HH #define KALMAN_TRACK_HH #include #include #include #include "TMath.h" #include "TMatrixD.h" namespace MAUS { namespace Kalman { // Class and Function Declarations class State; class TrackPoint; class Track; class TrackFit; /** @brief Basic Kalman State object * * It stores a state vector and associated covariance matrix object. * This is the simplest state object. * * Users can choose to set the "HasValue" flag to false (default if no vector set) * so that the filtering algorithm ignores the values. */ class State { // Make these friends so they can access the private member variables. // Faster computation! friend class Track; friend class TrackFit; friend State CalculateResidual(const State& st1, const State& st2); friend State CalculateFilteredResidual(const State& data, const State& measured); friend State CalculateChiSquaredResidual(const State& data, const State& measured); friend double CalculateChiSquaredUpdate(const State& st); public: /** @brief Initialise an empty state of the requred dimension and position */ explicit State(unsigned int dimension); /** @brief Initialise state with vector and covariance matrix */ State(TMatrixD vector, TMatrixD covariance); /** @brief Copy Constructor */ State(const State& st); /** @brief Assignment Operator */ State& operator=(const State& st); /** @brief Destructor */ virtual ~State() {} /** @brief Return the state vector */ TMatrixD& GetVector() { return _vector; } const TMatrixD& GetVector() const { return _vector; } /** @brief Set the state vector */ void SetVector(TMatrixD vec); /** @brief Return a reference to the covariance matrix */ TMatrixD& GetCovariance() { return _covariance; } const TMatrixD& GetCovariance() const { return _covariance; } /** @brief Set the covariance matrix */ void SetCovariance(TMatrixD cov); /** @brief Returns true if there is a stored value. */ bool HasValue() const { return _has_value; } /** @brief Set whether or not there is a stored value. * * This is useful in track fitting if a measurement plane has not * actually recorded a measurement. With this option set to false * the track fit will continue as expected and return an estimate * of the state vector for this state, without using any of the * stored information. */ void SetHasValue(bool HV) { _has_value = HV; } /** @brief Return the dimension of the state vector */ unsigned int GetDimension() const { return _dimension; } /** @brief Boolean operator, copies HasValue() function */ operator bool() const { return _has_value; } protected: private: unsigned int _dimension; TMatrixD _vector; TMatrixD _covariance; bool _has_value; }; /** @brief Kalman TrackPoint object * * This object stores all the information required for single point on a track. * The position and trackpoint ID is stored, additionally each of the predicted, filtered and * smoothed states (and covariances) are stored. * * Note: * - The TrackPoint::_id variable *MUST* correspond to measurement object ID, as stored in the * KalmanTrackFit class. This is how the algorithm performs the correct measurements. * - The State::_hasValue variables is used to indicate whether the data component of the * trackpoint is useful or an empty value. Only if the trackpoint contains a data value * will it be used in the filtering stage. */ class TrackPoint { public : /** @brief Initialise an empty trackpoint of the requred dimension and position */ TrackPoint(unsigned int dimension, unsigned int measurement_dim, double position, int id = 0); /** @brief Initialise an empty trackpoint with a measurement */ TrackPoint(unsigned int dimension, double position, State data, int id = 0); /** @brief Initialise trackpoint with all kalman states + data */ TrackPoint(State pred, State filt, State smoo, State data, double position, int id = 0 ); /** @brief Copy Constructor */ TrackPoint(const TrackPoint& trackpoint); /** @brief Assignment Operator */ TrackPoint& operator=(const TrackPoint& tp); /** @brief Shallow Copy function. Maintains previous ID and Position */ TrackPoint& copy(TrackPoint tp); /** @brief Destructor */ virtual ~TrackPoint() {} unsigned int GetDimension() const { return this->_predicted.GetDimension(); } /** @brief Get ID of trackpoint */ int GetId() const { return _id; } /** @brief Set ID of trackpoint. Should be unique! */ void SetId(int new_id) { _id = new_id; } /** @brief Return the position of the trackpoint */ double GetPosition() const { return _position; } /** @brief Set the position of the trackpoint */ void SetPosition(double new_pos) { _position = new_pos; } /** @brief Return the predicted state */ State& GetPredicted() { return _predicted; } const State& GetPredicted() const { return _predicted; } /** @brief Set the predicted state */ void SetPredicted(State state) { _predicted = state; } /** @brief Return the filtered state if available, otherwise return predicted */ State& GetFiltered(); const State& GetFiltered() const; /** @brief Set the filtered state */ void SetFiltered(State state) { _filtered = state; } /** @brief Return the smoothed state if available */ State& GetSmoothed(); const State& GetSmoothed() const; /** @brief Set the smoothed state */ void SetSmoothed(State state) { _smoothed = state; } /** @brief Return the data state */ State& GetData() { return _data; } const State& GetData() const { return _data; } /** @brief Set the data state */ void SetData(State state) { _data = state; } /** @brief Shortcut to the data->HasVaiue() function */ bool HasData() const { return _data.HasValue(); } protected : private: State _predicted; State _filtered; State _smoothed; State _data; int _id; double _position; }; /** @brief Stores a vector of trackpoints to make a track */ class Track { typedef std::vector< TrackPoint > TrackPointArray; public: /** @brief Initialise track with required dimension */ Track(unsigned int dimension, unsigned int length = 0); /** @brief Return the state found at specified index in state array */ TrackPoint GetTrackPoint(unsigned int index) const { return _track_vector[index]; } /** @brief Set the state at the specified index in the state array */ void SetTrackPoint(unsigned int index, TrackPoint tp); /** @brief Get a reference to the state at the specified index */ TrackPoint& operator[] (unsigned int index) { return _track_vector[index]; } /** @brief Get a const reference to the state at the specified index */ const TrackPoint& operator[] (unsigned int index) const { return _track_vector[index]; } /** @brief Get a const reference to the first track point */ TrackPoint& GetFirstTrackPoint() { return _track_vector[0]; } /** @brief Get a const reference to the first track point */ const TrackPoint& GetFirstTrackPoint() const { return _track_vector[0]; } /** @brief Get a reference to the last track point */ TrackPoint& GetLastTrackPoint() { return _track_vector[_track_vector.size()-1]; } /** @brief Get a reference to the last track point */ const TrackPoint& GetLastTrackPoint() const { return _track_vector[_track_vector.size()-1]; } /** @brief Return the number of states in the state array */ unsigned int GetLength() const { return _track_vector.size(); } /** @brief Append a state to the state array */ void Append(TrackPoint tp); /** @brief Remove a state from the specified index in the state array */ void DeleteTrackPoint(unsigned int index); /** @brief (Re)initialise the track using the structure of a similar track */ void Reset(const Track& similar_track); /** @brief Return the dimension of the state vectors */ unsigned int GetDimension() const { return _dimension; } protected: private: unsigned int _dimension; TrackPointArray _track_vector; }; } // namespace Kalman } // namespace MAUS /** Streamer operator for TMatrixD */ std::ostream& operator<<(std::ostream& out, const TMatrixD& matrix); /** Streamer operator for State */ std::ostream& operator<<(std::ostream& out, const MAUS::Kalman::State& state); /** Streamer operator for Track */ std::ostream& operator<<(std::ostream& out, const MAUS::Kalman::TrackPoint& track_point); /** Streamer operator for Track */ std::ostream& operator<<(std::ostream& out, const MAUS::Kalman::Track& track); #endif