From 8654751fcdf8e77f2e73bc926050b7538d38d24f Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 2 Sep 2025 22:12:18 -0400 Subject: [PATCH 01/35] Added kalman library stuff and start to outline new vision filter structure --- new_vision_filter/CMakeLists.txt | 26 ++ .../include/kalman/ExtendedKalmanFilter.hpp | 154 +++++++++ .../include/kalman/KalmanFilterBase.hpp | 80 +++++ .../kalman/LinearizedMeasurementModel.hpp | 79 +++++ .../include/kalman/LinearizedSystemModel.hpp | 80 +++++ new_vision_filter/include/kalman/Matrix.hpp | 160 +++++++++ .../include/kalman/MeasurementModel.hpp | 67 ++++ .../include/kalman/SquareRootBase.hpp | 87 +++++ .../kalman/SquareRootExtendedKalmanFilter.hpp | 280 ++++++++++++++++ .../include/kalman/SquareRootFilterBase.hpp | 46 +++ .../SquareRootUnscentedKalmanFilter.hpp | 317 ++++++++++++++++++ .../include/kalman/StandardBase.hpp | 90 +++++ .../include/kalman/StandardFilterBase.hpp | 46 +++ .../include/kalman/SystemModel.hpp | 69 ++++ new_vision_filter/include/kalman/Types.hpp | 75 +++++ .../include/kalman/UnscentedKalmanFilter.hpp | 275 +++++++++++++++ .../kalman/UnscentedKalmanFilterBase.hpp | 232 +++++++++++++ new_vision_filter/package.xml | 21 ++ new_vision_filter/src/camera.cpp | 25 ++ new_vision_filter/src/camera.hpp | 0 new_vision_filter/src/kick/basic_chip.hpp | 1 + new_vision_filter/src/kick/basic_kick.hpp | 1 + new_vision_filter/src/tracks/ball_track.hpp | 0 new_vision_filter/src/tracks/robot_track.hpp | 0 new_vision_filter/src/vision_filter_node.cpp | 35 ++ 25 files changed, 2246 insertions(+) create mode 100644 new_vision_filter/CMakeLists.txt create mode 100644 new_vision_filter/include/kalman/ExtendedKalmanFilter.hpp create mode 100644 new_vision_filter/include/kalman/KalmanFilterBase.hpp create mode 100644 new_vision_filter/include/kalman/LinearizedMeasurementModel.hpp create mode 100644 new_vision_filter/include/kalman/LinearizedSystemModel.hpp create mode 100644 new_vision_filter/include/kalman/Matrix.hpp create mode 100644 new_vision_filter/include/kalman/MeasurementModel.hpp create mode 100644 new_vision_filter/include/kalman/SquareRootBase.hpp create mode 100644 new_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp create mode 100644 new_vision_filter/include/kalman/SquareRootFilterBase.hpp create mode 100644 new_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp create mode 100644 new_vision_filter/include/kalman/StandardBase.hpp create mode 100644 new_vision_filter/include/kalman/StandardFilterBase.hpp create mode 100644 new_vision_filter/include/kalman/SystemModel.hpp create mode 100644 new_vision_filter/include/kalman/Types.hpp create mode 100644 new_vision_filter/include/kalman/UnscentedKalmanFilter.hpp create mode 100644 new_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp create mode 100644 new_vision_filter/package.xml create mode 100644 new_vision_filter/src/camera.cpp create mode 100644 new_vision_filter/src/camera.hpp create mode 100644 new_vision_filter/src/kick/basic_chip.hpp create mode 100644 new_vision_filter/src/kick/basic_kick.hpp create mode 100644 new_vision_filter/src/tracks/ball_track.hpp create mode 100644 new_vision_filter/src/tracks/robot_track.hpp create mode 100644 new_vision_filter/src/vision_filter_node.cpp diff --git a/new_vision_filter/CMakeLists.txt b/new_vision_filter/CMakeLists.txt new file mode 100644 index 000000000..12262f01e --- /dev/null +++ b/new_vision_filter/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(new_vision_filter) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/new_vision_filter/include/kalman/ExtendedKalmanFilter.hpp b/new_vision_filter/include/kalman/ExtendedKalmanFilter.hpp new file mode 100644 index 000000000..6441e9bb4 --- /dev/null +++ b/new_vision_filter/include/kalman/ExtendedKalmanFilter.hpp @@ -0,0 +1,154 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_EXTENDEDKALMANFILTER_HPP_ +#define KALMAN_EXTENDEDKALMANFILTER_HPP_ + +#include "KalmanFilterBase.hpp" +#include "StandardFilterBase.hpp" +#include "LinearizedSystemModel.hpp" +#include "LinearizedMeasurementModel.hpp" + +namespace Kalman { + + /** + * @brief Extended Kalman Filter (EKF) + * + * This implementation is based upon [An Introduction to the Kalman Filter](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) + * by Greg Welch and Gary Bishop. + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class ExtendedKalmanFilter : public KalmanFilterBase, + public StandardFilterBase + { + public: + //! Kalman Filter base type + typedef KalmanFilterBase KalmanBase; + //! Standard Filter base type + typedef StandardFilterBase StandardBase; + + //! Numeric Scalar Type inherited from base + using typename KalmanBase::T; + + //! State Type inherited from base + using typename KalmanBase::State; + + //! Linearized Measurement Model Type + template class CovarianceBase> + using MeasurementModelType = LinearizedMeasurementModel; + + //! Linearized System Model Type + template class CovarianceBase> + using SystemModelType = LinearizedSystemModel; + + protected: + //! Kalman Gain Matrix Type + template + using KalmanGain = Kalman::KalmanGain; + + protected: + //! State Estimate + using KalmanBase::x; + //! State Covariance Matrix + using StandardBase::P; + + public: + /** + * @brief Constructor + */ + ExtendedKalmanFilter() + { + // Setup state and covariance + P.setIdentity(); + } + + /** + * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) + * + * @param [in] s The System model + * @return The updated state estimate + */ + template class CovarianceBase> + const State& predict( SystemModelType& s ) + { + // predict state (without control) + Control u; + u.setZero(); + return predict( s, u ); + } + + /** + * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model + * + * @param [in] s The System model + * @param [in] u The Control input vector + * @return The updated state estimate + */ + template class CovarianceBase> + const State& predict( SystemModelType& s, const Control& u ) + { + s.updateJacobians( x, u ); + + // predict state + x = s.f(x, u); + + // predict covariance + P = ( s.F * P * s.F.transpose() ) + ( s.W * s.getCovariance() * s.W.transpose() ); + + // return state prediction + return this->getState(); + } + + /** + * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model + * + * @param [in] m The Measurement model + * @param [in] z The measurement vector + * @return The updated state estimate + */ + template class CovarianceBase> + const State& update( MeasurementModelType& m, const Measurement& z ) + { + m.updateJacobians( x ); + + // COMPUTE KALMAN GAIN + // compute innovation covariance + Covariance S = ( m.H * P * m.H.transpose() ) + ( m.V * m.getCovariance() * m.V.transpose() ); + + // compute kalman gain + KalmanGain K = P * m.H.transpose() * S.inverse(); + + // UPDATE STATE ESTIMATE AND COVARIANCE + // Update state using computed kalman gain and innovation + x += K * ( z - m.h( x ) ); + + // Update covariance + P -= K * m.H * P; + + // return updated state estimate + return this->getState(); + } + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/KalmanFilterBase.hpp b/new_vision_filter/include/kalman/KalmanFilterBase.hpp new file mode 100644 index 000000000..ffe5faeab --- /dev/null +++ b/new_vision_filter/include/kalman/KalmanFilterBase.hpp @@ -0,0 +1,80 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_KALMANFILTERBASE_HPP_ +#define KALMAN_KALMANFILTERBASE_HPP_ + +#include "Matrix.hpp" +#include "Types.hpp" + +namespace Kalman { + + /** + * @brief Abstract base class for all Kalman Filters + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class KalmanFilterBase + { + public: + static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/StateType::RowsAtCompileTime > 0, + "State vector must contain at least 1 element" /* or be dynamic */); + static_assert(StateType::ColsAtCompileTime == 1, "State type must be a column vector"); + + //! Numeric scalar type + typedef typename StateType::Scalar T; + + //! Type of the state vector + typedef StateType State; + + protected: + //! Estimated state + State x; + + public: + /** + * Get current state estimate + */ + const State& getState() const + { + return x; + } + + /** + * @brief Initialize state + * @param initialState The initial state of the system + */ + void init(const State& initialState) + { + x = initialState; + } + protected: + /** + * @brief Protected constructor + */ + KalmanFilterBase() + { + } + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/LinearizedMeasurementModel.hpp b/new_vision_filter/include/kalman/LinearizedMeasurementModel.hpp new file mode 100644 index 000000000..e3831f1d4 --- /dev/null +++ b/new_vision_filter/include/kalman/LinearizedMeasurementModel.hpp @@ -0,0 +1,79 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_LINEARIZEDMEASUREMENTMODEL_HPP_ +#define KALMAN_LINEARIZEDMEASUREMENTMODEL_HPP_ + +#include "MeasurementModel.hpp" + +namespace Kalman { + template + class ExtendedKalmanFilter; + template + class SquareRootExtendedKalmanFilter; + + /** + * @brief Abstract base class of all linearized (first order taylor expansion) measurement models + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + * @param MeasurementType The vector-type of the measurement (usually some type derived from Kalman::Vector) + * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) + */ + template class CovarianceBase = StandardBase> + class LinearizedMeasurementModel : public MeasurementModel + { + friend class ExtendedKalmanFilter; + friend class SquareRootExtendedKalmanFilter; + public: + //! Measurement model base + typedef MeasurementModel Base; + + //! System state type + using typename Base::State; + + //! Measurement vector type + using typename Base::Measurement; + + protected: + //! Measurement model jacobian + Jacobian H; + //! Measurement model noise jacobian + Jacobian V; + + /** + * Callback function for state-dependent update of Jacobi-matrices H and V before each update step + */ + virtual void updateJacobians( const State& x ) + { + // No update by default + (void)x; + } + protected: + LinearizedMeasurementModel() + { + H.setIdentity(); + V.setIdentity(); + } + ~LinearizedMeasurementModel() {} + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/LinearizedSystemModel.hpp b/new_vision_filter/include/kalman/LinearizedSystemModel.hpp new file mode 100644 index 000000000..836cc91cb --- /dev/null +++ b/new_vision_filter/include/kalman/LinearizedSystemModel.hpp @@ -0,0 +1,80 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_LINEARIZEDSYSTEMMODEL_HPP_ +#define KALMAN_LINEARIZEDSYSTEMMODEL_HPP_ + +#include "SystemModel.hpp" + +namespace Kalman { + template + class ExtendedKalmanFilter; + template + class SquareRootExtendedKalmanFilter; + + /** + * @brief Abstract base class of all linearized (first order taylor expansion) system models + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + * @param ControlType The vector-type of the control input (usually some type derived from Kalman::Vector) + * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) + */ + template, template class CovarianceBase = StandardBase > + class LinearizedSystemModel : public SystemModel + { + friend class ExtendedKalmanFilter; + friend class SquareRootExtendedKalmanFilter; + public: + //! System model base + typedef SystemModel Base; + + //! System state type + using typename Base::State; + + //! System control input type + using typename Base::Control; + + protected: + //! System model jacobian + Jacobian F; + //! System model noise jacobian + Jacobian W; + + /** + * Callback function for state-dependent update of Jacobi-matrices F and W before each update step + */ + virtual void updateJacobians( const State& x, const Control& u ) + { + // No update by default + (void)x; + (void)u; + } + protected: + LinearizedSystemModel() + { + F.setIdentity(); + W.setIdentity(); + } + ~LinearizedSystemModel() {} + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/Matrix.hpp b/new_vision_filter/include/kalman/Matrix.hpp new file mode 100644 index 000000000..574ebdf58 --- /dev/null +++ b/new_vision_filter/include/kalman/Matrix.hpp @@ -0,0 +1,160 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_MATRIX_HPP_ +#define KALMAN_MATRIX_HPP_ + +#include + +#include + +#define KALMAN_VECTOR(NAME, T, N) \ + typedef Kalman::Vector Base; \ + using typename Base::Scalar; \ + using Base::RowsAtCompileTime; \ + using Base::ColsAtCompileTime; \ + using Base::SizeAtCompileTime; \ + \ + NAME(void) : Kalman::Vector() {} \ + \ + template \ + NAME(const Eigen::MatrixBase& other) : Kalman::Vector(other) {} \ + \ + template \ + NAME& operator= (const Eigen::MatrixBase & other) \ + { \ + this->Base::operator=(other); \ + return *this; \ + } + +namespace Kalman { + const int Dynamic = Eigen::Dynamic; + + /** + * @class Kalman::Matrix + * @brief Template type for matrices + * @param T The numeric scalar type + * @param rows The number of rows + * @param cols The number of columns + */ + template + using Matrix = Eigen::Matrix; + + /** + * @brief Template type for vectors + * @param T The numeric scalar type + * @param N The vector dimension + */ + template + class Vector : public Matrix + { + public: + //! Matrix base type + typedef Matrix Base; + + using typename Base::Scalar; + using Base::RowsAtCompileTime; + using Base::ColsAtCompileTime; + using Base::SizeAtCompileTime; + + Vector(void) : Matrix() {} + + /** + * @brief Copy constructor + */ + template + Vector(const Eigen::MatrixBase& other) + : Matrix(other) + { } + /** + * @brief Copy assignment constructor + */ + template + Vector& operator= (const Eigen::MatrixBase & other) + { + this->Base::operator=(other); + return *this; + } + }; + + /** + * @brief Cholesky square root decomposition of a symmetric positive-definite matrix + * @param _MatrixType The matrix type + * @param _UpLo Square root form (Eigen::Lower or Eigen::Upper) + */ + template + class Cholesky : public Eigen::LLT< _MatrixType, _UpLo > + { + public: + Cholesky() : Eigen::LLT< _MatrixType, _UpLo >() {} + + /** + * @brief Construct cholesky square root decomposition from matrix + * @param m The matrix to be decomposed + */ + Cholesky(const _MatrixType& m ) : Eigen::LLT< _MatrixType, _UpLo >(m) {} + + /** + * @brief Set decomposition to identity + */ + Cholesky& setIdentity() + { + this->m_matrix.setIdentity(); + this->m_isInitialized = true; + return *this; + } + + /** + * @brief Check whether the decomposed matrix is the identity matrix + */ + bool isIdentity() const + { + eigen_assert(this->m_isInitialized && "LLT is not initialized."); + return this->m_matrix.isIdentity(); + } + + /** + * @brief Set lower triangular part of the decomposition + * @param matrix The lower part stored in a full matrix + */ + template + Cholesky& setL(const Eigen::MatrixBase & matrix) + { + this->m_matrix = matrix.template triangularView(); + this->m_isInitialized = true; + return *this; + } + + /** + * @brief Set upper triangular part of the decomposition + * @param matrix The upper part stored in a full matrix + */ + template + Cholesky& setU(const Eigen::MatrixBase & matrix) + { + this->m_matrix = matrix.template triangularView().adjoint(); + this->m_isInitialized = true; + return *this; + } + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/MeasurementModel.hpp b/new_vision_filter/include/kalman/MeasurementModel.hpp new file mode 100644 index 000000000..b3f3b3754 --- /dev/null +++ b/new_vision_filter/include/kalman/MeasurementModel.hpp @@ -0,0 +1,67 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_MEASUREMENTMODEL_HPP_ +#define KALMAN_MEASUREMENTMODEL_HPP_ + +#include + +#include "StandardBase.hpp" + +namespace Kalman { + /** + * @brief Abstract base class of all measurement models + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + * @param MeasurementType The vector-type of the measurement (usually some type derived from Kalman::Vector) + * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) + */ + template class CovarianceBase = StandardBase> + class MeasurementModel : public CovarianceBase + { + static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/StateType::RowsAtCompileTime > 0, + "State vector must contain at least 1 element" /* or be dynamic */); + static_assert(/*MeasurementType::RowsAtCompileTime == Dynamic ||*/MeasurementType::RowsAtCompileTime > 0, + "Measurement vector must contain at least 1 element" /* or be dynamic */); + static_assert(std::is_same::value, + "State and Measurement scalar types must be identical"); + public: + //! System state type + typedef StateType State; + + //! Measurement vector type + typedef MeasurementType Measurement; + + public: + /** + * Measurement Model Function h + * + * Predicts the estimated measurement value given the current state estimate x + */ + virtual Measurement h(const State& x) const = 0; + + protected: + MeasurementModel() {} + virtual ~MeasurementModel() {} + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/SquareRootBase.hpp b/new_vision_filter/include/kalman/SquareRootBase.hpp new file mode 100644 index 000000000..a2312b312 --- /dev/null +++ b/new_vision_filter/include/kalman/SquareRootBase.hpp @@ -0,0 +1,87 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_SQUAREROOTBASE_HPP_ +#define KALMAN_SQUAREROOTBASE_HPP_ + +#include "Types.hpp" + +namespace Kalman { + + /** + * @brief Abstract base class for square-root filters and models + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class SquareRootBase + { + protected: + //! Covariance Square Root + CovarianceSquareRoot S; + + public: + /** + * Get covariance (as square root) + */ + const CovarianceSquareRoot& getCovarianceSquareRoot() const + { + return S; + } + + /** + * Get covariance reconstructed from square root + */ + Covariance getCovariance() const + { + return S.reconstructedMatrix(); + } + + /** + * Set Covariance + */ + bool setCovariance(const Covariance& covariance) + { + S.compute(covariance); + return (S.info() == Eigen::Success); + } + + /** + * @brief Set Covariance using Square Root + * + * @param covSquareRoot Lower triangular Matrix representing the covariance + * square root (i.e. P = LLˆT) + */ + bool setCovarianceSquareRoot(const Covariance& covSquareRoot) + { + S.setL(covSquareRoot); + return true; + } + + protected: + SquareRootBase() + { + S.setIdentity(); + } + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp b/new_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp new file mode 100644 index 000000000..2f876af57 --- /dev/null +++ b/new_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp @@ -0,0 +1,280 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_SQUAREROOTEXTENDEDKALMANFILTER_HPP_ +#define KALMAN_SQUAREROOTEXTENDEDKALMANFILTER_HPP_ + +#include "KalmanFilterBase.hpp" +#include "SquareRootFilterBase.hpp" +#include "LinearizedSystemModel.hpp" +#include "LinearizedMeasurementModel.hpp" + +namespace Kalman { + + /** + * @brief Square-Root Extended Kalman Filter (SR-EKF) + * + * This implementation is based upon [An Introduction to the Kalman Filter](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) + * by Greg Welch and Gary Bishop. + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class SquareRootExtendedKalmanFilter : public KalmanFilterBase, + public SquareRootFilterBase + { + public: + //! Kalman Filter base type + typedef KalmanFilterBase KalmanBase; + //! SquareRoot Filter base type + typedef SquareRootFilterBase SquareRootBase; + + //! Numeric Scalar Type inherited from base + using typename KalmanBase::T; + + //! State Type inherited from base + using typename KalmanBase::State; + + //! Linearized Measurement Model Type + template class CovarianceBase> + using MeasurementModelType = LinearizedMeasurementModel; + + //! Linearized System Model Type + template class CovarianceBase> + using SystemModelType = LinearizedSystemModel; + + protected: + //! Kalman Gain Matrix Type + template + using KalmanGain = Kalman::KalmanGain; + + protected: + //! State estimate + using KalmanBase::x; + //! State covariance square root + using SquareRootBase::S; + + public: + /** + * @brief Constructor + */ + SquareRootExtendedKalmanFilter() + { + // Setup covariance + S.setIdentity(); + } + + /** + * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) + * + * @param [in] s The System model + * @return The updated state estimate + */ + template class CovarianceBase> + const State& predict( SystemModelType& s ) + { + // predict state (without control) + Control u; + u.setZero(); + return predict( s, u ); + } + + /** + * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model + * + * @param [in] s The System model + * @param [in] u The Control input vector + * @return The updated state estimate + */ + template class CovarianceBase> + const State& predict( SystemModelType& s, const Control& u ) + { + s.updateJacobians( x, u ); + + // predict state + x = s.f(x, u); + + // predict covariance + computePredictedCovarianceSquareRoot(s.F, S, s.W, s.getCovarianceSquareRoot(), S); + + // return state prediction + return this->getState(); + } + + /** + * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model + * + * @param [in] m The Measurement model + * @param [in] z The measurement vector + * @return The updated state estimate + */ + template class CovarianceBase> + const State& update( MeasurementModelType& m, const Measurement& z ) + { + m.updateJacobians( x ); + + // COMPUTE KALMAN GAIN + // compute innovation covariance + CovarianceSquareRoot S_y; + computePredictedCovarianceSquareRoot(m.H, S, m.V, m.getCovarianceSquareRoot(), S_y); + + // compute kalman gain + KalmanGain K; + computeKalmanGain(m.H, S_y, K); + + // UPDATE STATE ESTIMATE AND COVARIANCE + // Update state using computed kalman gain and innovation + x += K * ( z - m.h( x ) ); + + // Update covariance + updateStateCovariance(K, m.H); + + // return updated state estimate + return this->getState(); + } + protected: + + /** + * @brief Compute the predicted state or innovation covariance (as square root) + * + * The formula for computing the propagated square root covariance matrix can be + * deduced in a very straight forward way using the method outlined in the + * [Unscented Kalman Filter Tutorial](https://cse.sc.edu/~terejanu/files/tutorialUKF.pdf) + * by Gabriel A. Terejanu for the UKF (Section 3, formulas (27) and (28)). + * + * Starting from the standard update formula + * + * \f[ \hat{P} = FPF^T + WQW^T \f] + * + * and using the square-root decomposition \f$ P = SS^T \f$ with \f$S\f$ being lower-triagonal + * as well as the (lower triangular) square root \f$\sqrt{Q}\f$ of \f$Q\f$ this can be formulated as + * + * \f{align*}{ + * \hat{P} &= FSS^TF^T + W\sqrt{Q}\sqrt{Q}^TW^T \\ + * &= FS(FS)^T + W\sqrt{Q}(W\sqrt{Q})^T \\ + * &= \begin{bmatrix} FS & W\sqrt{Q} \end{bmatrix} + * \begin{bmatrix} (FS)^T \\ (W\sqrt{Q})^T \end{bmatrix} + * \f} + * + * The blockmatrix + * + * \f[ \begin{bmatrix} (FS)^T \\ (W\sqrt{Q})^T \end{bmatrix} \in \mathbb{R}^{2n \times n} \f] + * + * can then be decomposed into a product of matrices \f$OR\f$ with \f$O\f$ being orthogonal + * and \f$R\f$ being upper triangular (also known as QR decompositon). Using this \f$\hat{P}\f$ + * can be written as + * + * \f{align*}{ + * \hat{P} &= \begin{bmatrix} FS & W\sqrt{Q} \end{bmatrix} + * \begin{bmatrix} (FS)^T \\ (W\sqrt{Q})^T \end{bmatrix} \\ + * &= (OR)^TOR \\ + * &= R^T \underbrace{O^T O}_{=I}R \\ + * &= LL^T \qquad \text{ with } L := R^T + * \f} + * + * Thus the lower-triangular square root of \f$\hat{P}\f$ is equaivalent to the transpose + * of the upper-triangular matrix obtained from QR-decompositon of the augmented block matrix + * + * \f[ \begin{bmatrix} FS & W\sqrt{Q} \end{bmatrix}^T = \begin{bmatrix} S^T F^T \\ \sqrt{Q}^T W^T \end{bmatrix} \in \mathbb{R}^{2n \times n} \f] + * + * The same can be applied for the innovation covariance by replacing the jacobians and + * the noise covariance accordingly. + * + * @param [in] A The jacobian of state transition or measurement function w.r.t. state or measurement + * @param [in] S The state covariance (as square root) + * @param [in] B The jacobian of state transition or measurement function w.r.t. state or measurement + * @param [in] R The system model or measurement noise covariance (as square root) + * @param [out] S_pred The predicted covariance (as square root) + * @return True on success, false on failure due to numerical issue + */ + template + bool computePredictedCovarianceSquareRoot( const Jacobian& A, const CovarianceSquareRoot& S, + const Jacobian& B, const CovarianceSquareRoot& R, + CovarianceSquareRoot& S_pred) + { + // Compute QR decomposition of (transposed) augmented matrix + Matrix tmp; + tmp.template topRows() = S.matrixU() * A.transpose(); + tmp.template bottomRows() = R.matrixU() * B.transpose(); + + // TODO: Use ColPivHouseholderQR + Eigen::HouseholderQR qr( tmp ); + + // Set S_pred matrix as upper triangular square root + S_pred.setU(qr.matrixQR().template topRightCorner()); + return true; + } + + /** + * @brief Compute Kalman gain from state covariance, innovation covariance and measurement jacobian + * + * The formula for the kalman gain using square-root covariances can be deduced from the + * original update formula + * + * \f[ K = P H^T P_{yy}^{-1} \Leftrightarrow K P_{yy} = P H^T \f] + * + * with \f$P_yy\f$ being the innovation covariance. With \f$P = SS^T\f$ and \f$P_yy = S_yS_y^T\f$ + * and transposition of the whole equation the above can be formulated as + * + * \f{align*}{ + * K (S_yS_y^T) &= SS^TH^T \\ + * (K(S_yS_y^T))^T &= (SS^TH^T)^T \\ + * (S_yS_y^T)^TK^T &= H(SS^T)^T \\ + * \underbrace{S_yS_y^T}_{=P_{yy}} K^T &= HSS^T + * \f} + * + * Since \f$S_y\f$ is lower triangular, the above can be solved using backsubstitution + * in an efficient manner. + * + * @param [in] H The jacobian of the measurement function w.r.t. the state + * @param [in] S_y The Innovation covariance square root + * @param [out] K The computed Kalman Gain + */ + template + bool computeKalmanGain( const Jacobian& H, + const CovarianceSquareRoot& S_y, + KalmanGain& K) + { + // Solve using backsubstitution + // AX=B with B = HSS^T and X = K^T and A = S_yS_y^T + K = S_y.solve(H*S.reconstructedMatrix()).transpose(); + return true; + } + + /** + * @brief Update state covariance using Kalman gain + * + * @param [in] K The Kalman gain + * @param [in] H The jacobian of the measurement function w.r.t. the state + */ + template + bool updateStateCovariance( const KalmanGain& K, + const Jacobian& H) + { + // TODO: update covariance without using decomposition + Matrix P = S.reconstructedMatrix(); + S.compute( (P - K * H * P).eval() ); + return (S.info() == Eigen::Success); + } + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/SquareRootFilterBase.hpp b/new_vision_filter/include/kalman/SquareRootFilterBase.hpp new file mode 100644 index 000000000..12d5a4271 --- /dev/null +++ b/new_vision_filter/include/kalman/SquareRootFilterBase.hpp @@ -0,0 +1,46 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_SQUAREROOTFILTERBASE_HPP_ +#define KALMAN_SQUAREROOTFILTERBASE_HPP_ + +#include "SquareRootBase.hpp" + +namespace Kalman { + + /** + * @brief Abstract base class for square root filters + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class SquareRootFilterBase : public SquareRootBase + { + protected: + //! SquareRoot Base Type + typedef SquareRootBase Base; + + //! Covariance Square Root + using Base::S; + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp b/new_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp new file mode 100644 index 000000000..f23626732 --- /dev/null +++ b/new_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp @@ -0,0 +1,317 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_SQUAREROOTUNSCENTEDKALMANFILTER_HPP_ +#define KALMAN_SQUAREROOTUNSCENTEDKALMANFILTER_HPP_ + +#include "UnscentedKalmanFilterBase.hpp" +#include "SquareRootFilterBase.hpp" + +namespace Kalman { + + /** + * @brief Square Root Unscented Kalman Filter (SR-UKF) + * + * @note This is the square-root implementation variant of UnscentedKalmanFilter + * + * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. + * Whenever "the paper" is referenced within this file then this paper is meant. + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class SquareRootUnscentedKalmanFilter : public UnscentedKalmanFilterBase, + public SquareRootFilterBase + { + public: + //! Unscented Kalman Filter base type + typedef UnscentedKalmanFilterBase UnscentedBase; + + //! Square Root Filter base type + typedef SquareRootFilterBase SquareRootBase; + + //! Numeric Scalar Type inherited from base + using typename UnscentedBase::T; + + //! State Type inherited from base + using typename UnscentedBase::State; + + //! Measurement Model Type + template class CovarianceBase> + using MeasurementModelType = typename UnscentedBase::template MeasurementModelType; + + //! System Model Type + template class CovarianceBase> + using SystemModelType = typename UnscentedBase::template SystemModelType; + + protected: + //! The number of sigma points (depending on state dimensionality) + using UnscentedBase::SigmaPointCount; + + //! Matrix type containing the sigma state or measurement points + template + using SigmaPoints = typename UnscentedBase::template SigmaPoints; + + //! Kalman Gain Matrix Type + template + using KalmanGain = Kalman::KalmanGain; + + protected: + // Member variables + + //! State Estimate + using UnscentedBase::x; + + //! Square Root of State Covariance + using SquareRootBase::S; + + //! Sigma points (state) + using UnscentedBase::sigmaStatePoints; + + public: + /** + * Constructor + * + * See paper for detailed parameter explanation + * + * @param alpha Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) + * @param beta Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) + * @param kappa Secondary scaling parameter (usually 0) + */ + SquareRootUnscentedKalmanFilter(T alpha = T(1), T beta = T(2), T kappa = T(0)) + : UnscentedKalmanFilterBase(alpha, beta, kappa) + { + // Init covariance to identity + S.setIdentity(); + } + + /** + * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) + * + * @param [in] s The System model + * @return The updated state estimate + */ + template class CovarianceBase> + const State& predict( const SystemModelType& s ) + { + // predict state (without control) + Control u; + u.setZero(); + return predict( s, u ); + } + + /** + * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model + * + * @param [in] s The System model + * @param [in] u The Control input vector + * @return The updated state estimate + */ + template class CovarianceBase> + const State& predict( const SystemModelType& s, const Control& u ) + { + // Compute sigma points + computeSigmaPoints(); + + // Compute predicted state + x = this->template computeStatePrediction(s, u); + + // Compute predicted covariance + if(!computeCovarianceSquareRootFromSigmaPoints(x, sigmaStatePoints, s.getCovarianceSquareRoot(), S)) + { + // TODO: handle numerical error + assert(false); + } + + // Return predicted state + return this->getState(); + } + + /** + * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model + * + * @param [in] m The Measurement model + * @param [in] z The measurement vector + * @return The updated state estimate + */ + template class CovarianceBase> + const State& update( const MeasurementModelType& m, const Measurement& z ) + { + SigmaPoints sigmaMeasurementPoints; + + // Predict measurement (and corresponding sigma points) + Measurement y = this->template computeMeasurementPrediction(m, sigmaMeasurementPoints); + + // Compute square root innovation covariance + CovarianceSquareRoot S_y; + if(!computeCovarianceSquareRootFromSigmaPoints(y, sigmaMeasurementPoints, m.getCovarianceSquareRoot(), S_y)) + { + // TODO: handle numerical error + assert(false); + } + + KalmanGain K; + computeKalmanGain(y, sigmaMeasurementPoints, S_y, K); + + // Update state + x += K * ( z - y ); + + // Update state covariance + if(!updateStateCovariance(K, S_y)) + { + // TODO: handle numerical error + assert(false); + } + + return this->getState(); + } + + protected: + /** + * @brief Compute sigma points from current state estimate and state covariance + * + * @note This covers equations (17) and (22) of Algorithm 3.1 in the Paper + */ + bool computeSigmaPoints() + { + // Get square root of covariance + Matrix _S = S.matrixL().toDenseMatrix(); + + // Set left "block" (first column) + sigmaStatePoints.template leftCols<1>() = x; + // Set center block with x + gamma * S + sigmaStatePoints.template block(0,1) + = ( this->gamma * _S).colwise() + x; + // Set right block with x - gamma * S + sigmaStatePoints.template rightCols() + = (-this->gamma * _S).colwise() + x; + + return true; + } + + /** + * @brief Compute the Covariance Square root from sigma points and noise covariance + * + * @note This covers equations (20) and (21) as well as (25) and (26) of Algorithm 3.1 in the Paper + * + * @param [in] mean The mean predicted state or measurement + * @param [in] sigmaPoints the predicted sigma state or measurement points + * @param [in] noiseCov The system or measurement noise covariance (as square root) + * @param [out] cov The propagated state or innovation covariance (as square root) + * + * @return True on success, false if a numerical error is encountered when updating the matrix + */ + template + bool computeCovarianceSquareRootFromSigmaPoints(const Type& mean, const SigmaPoints& sigmaPoints, + const CovarianceSquareRoot& noiseCov, CovarianceSquareRoot& cov) + { + // Compute QR decomposition of (transposed) augmented matrix + Matrix tmp; + tmp.template topRows<2*State::RowsAtCompileTime>() = std::sqrt(this->sigmaWeights_c[1]) * ( sigmaPoints.template rightCols().colwise() - mean).transpose(); + tmp.template bottomRows() = noiseCov.matrixU().toDenseMatrix(); + + // TODO: Use ColPivHouseholderQR + Eigen::HouseholderQR qr( tmp ); + + // Set R matrix as upper triangular square root + cov.setU(qr.matrixQR().template topRightCorner()); + + // Perform additional rank 1 update + // TODO: According to the paper (Section 3, "Cholesky factor updating") the update + // is defined using the square root of the scalar, however the correct result + // is obtained when using the weight directly rather than using the square root + // It should be checked whether this is a bug in Eigen or in the Paper + // T nu = std::copysign( std::sqrt(std::abs(sigmaWeights_c[0])), sigmaWeights_c[0]); + T nu = this->sigmaWeights_c[0]; + cov.rankUpdate( sigmaPoints.template leftCols<1>() - mean, nu ); + + return (cov.info() == Eigen::Success); + } + + /** + * @brief Compute the Kalman Gain from predicted measurement and sigma points and the innovation covariance. + * + * @note This covers equations (27) and (28) of Algorithm 3.1 in the Paper + * + * We need to solve the equation \f$ K (S_y S_y^T) = P \f$ for \f$ K \f$ using backsubstitution. + * In order to apply standard backsubstitution using the `solve` method we must bring the + * equation into the form + * \f[ AX = B \qquad \text{with } A = S_yS_y^T \f] + * Thus we transpose the whole equation to obtain + * \f{align*}{ + * ( K (S_yS_y^T))^T &= P^T \Leftrightarrow \\ + * (S_yS_y^T)^T K^T &= P^T \Leftrightarrow \\ + * (S_yS_y^T) K^T &= P^T + *\f} + * Hence our \f$ X := K^T\f$ and \f$ B:= P^T \f$ + * + * @param [in] y The predicted measurement + * @param [in] sigmaMeasurementPoints The predicted sigma measurement points + * @param [in] S_y The innovation covariance as square-root + * @param [out] K The computed Kalman Gain matrix \f$ K \f$ + */ + template + bool computeKalmanGain( const Measurement& y, + const SigmaPoints& sigmaMeasurementPoints, + const CovarianceSquareRoot& S_y, + KalmanGain& K) + { + // Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs + // when Measurement::RowsAtCompileTime == 1 AND State::RowsAtCompileTime >= 8 + decltype(sigmaStatePoints) W = this->sigmaWeights_c.transpose().template replicate(); + Matrix P + = (sigmaStatePoints.colwise() - x).cwiseProduct( W ).eval() + * (sigmaMeasurementPoints.colwise() - y).transpose(); + + K = S_y.solve(P.transpose()).transpose(); + return true; + } + + /** + * @brief Update the state covariance matrix using the Kalman Gain and the Innovation Covariance + * + * @note This covers equations (29) and (30) of Algorithm 3.1 in the Paper + * + * @param [in] K The computed Kalman Gain matrix + * @param [in] S_y The innovation covariance as square-root + * @return True on success, false if a numerical error is encountered when updating the matrix + */ + template + bool updateStateCovariance(const KalmanGain& K, const CovarianceSquareRoot& S_y) + { + KalmanGain U = K * S_y.matrixL(); + for(int i = 0; i < U.cols(); ++i) + { + S.rankUpdate( U.col(i), -1 ); + + if( S.info() == Eigen::NumericalIssue ) + { + // TODO: handle numerical issues in some sensible way + return false; + } + } + + return true; + } + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/StandardBase.hpp b/new_vision_filter/include/kalman/StandardBase.hpp new file mode 100644 index 000000000..7d96d4b0a --- /dev/null +++ b/new_vision_filter/include/kalman/StandardBase.hpp @@ -0,0 +1,90 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_STANDARDBASE_HPP_ +#define KALMAN_STANDARDBASE_HPP_ + +#include "Types.hpp" + +namespace Kalman { + + /** + * @brief Abstract base class for standard (non-square root) filters and models + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class StandardBase + { + protected: + //! Covariance + Covariance P; + + public: + /** + * Get covariance + */ + const Covariance& getCovariance() const + { + return P; + } + + /** + * Get covariance (as square root) + */ + CovarianceSquareRoot getCovarianceSquareRoot() const + { + return CovarianceSquareRoot(P); + } + + /** + * Set Covariance + */ + bool setCovariance(const Covariance& covariance) + { + P = covariance; + return true; + } + + /** + * @brief Set Covariance using Square Root + * + * @param covSquareRoot Lower triangular Matrix representing the covariance + * square root (i.e. P = LLˆT) + */ + bool setCovarianceSquareRoot(const Covariance& covSquareRoot) + { + CovarianceSquareRoot S; + S.setL(covSquareRoot); + P = S.reconstructedMatrix(); + return true; + } + + + protected: + StandardBase() + { + P.setIdentity(); + } + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/StandardFilterBase.hpp b/new_vision_filter/include/kalman/StandardFilterBase.hpp new file mode 100644 index 000000000..03336f530 --- /dev/null +++ b/new_vision_filter/include/kalman/StandardFilterBase.hpp @@ -0,0 +1,46 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_STANDARDFILTERBASE_HPP_ +#define KALMAN_STANDARDFILTERBASE_HPP_ + +#include "StandardBase.hpp" + +namespace Kalman { + + /** + * @brief Abstract base class for standard (non-square root) filters + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class StandardFilterBase : public StandardBase + { + protected: + //! Standard Base Type + typedef StandardBase Base; + + //! Covariance matrix + using Base::P; + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/SystemModel.hpp b/new_vision_filter/include/kalman/SystemModel.hpp new file mode 100644 index 000000000..f20d73c6b --- /dev/null +++ b/new_vision_filter/include/kalman/SystemModel.hpp @@ -0,0 +1,69 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_SYSTEMMODEL_HPP_ +#define KALMAN_SYSTEMMODEL_HPP_ + +#include + +#include "Matrix.hpp" +#include "StandardBase.hpp" + +namespace Kalman { + /** + * @brief Abstract base class of all system models + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + * @param ControlType The vector-type of the control input (usually some type derived from Kalman::Vector) + * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) + */ + template, template class CovarianceBase = StandardBase> + class SystemModel : public CovarianceBase + { + static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/ StateType::RowsAtCompileTime > 0, + "State vector must contain at least 1 element" /* or be dynamic */); + static_assert(/*ControlType::RowsAtCompileTime == Dynamic ||*/ ControlType::RowsAtCompileTime >= 0, + "Control vector must contain at least 0 elements" /* or be dynamic */); + static_assert(std::is_same::value, + "State and Control scalar types must be identical"); + public: + //! System state type + typedef StateType State; + + //! System control input type + typedef ControlType Control; + + public: + /** + * @brief State Transition Function f + * + * Computes the predicted system state in the next timestep given + * the current state x and the control input u + */ + virtual State f(const State& x, const Control& u) const = 0; + + protected: + SystemModel() {} + virtual ~SystemModel() {} + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/Types.hpp b/new_vision_filter/include/kalman/Types.hpp new file mode 100644 index 000000000..38772a01c --- /dev/null +++ b/new_vision_filter/include/kalman/Types.hpp @@ -0,0 +1,75 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_TYPES_HPP_ +#define KALMAN_TYPES_HPP_ + +#include "Matrix.hpp" + +namespace Kalman +{ + /** + * @class Kalman::SquareMatrix + * @brief Template type representing a square matrix + * @param T The numeric scalar type + * @param N The dimensionality of the Matrix + */ + template + using SquareMatrix = Matrix; + + /** + * @class Kalman::Covariance + * @brief Template type for covariance matrices + * @param Type The vector type for which to generate a covariance (usually a state or measurement type) + */ + template + using Covariance = SquareMatrix; + + /** + * @class Kalman::CovarianceSquareRoot + * @brief Template type for covariance square roots + * @param Type The vector type for which to generate a covariance (usually a state or measurement type) + */ + template + using CovarianceSquareRoot = Cholesky< Covariance >; + + /** + * @class Kalman::KalmanGain + * @brief Template type of Kalman Gain + * @param State The system state type + * @param Measurement The measurement type + */ + template + using KalmanGain = Matrix; + + /** + * @class Kalman::Jacobian + * @brief Template type of jacobian of A w.r.t. B + */ + template + using Jacobian = Matrix; +} + +#endif diff --git a/new_vision_filter/include/kalman/UnscentedKalmanFilter.hpp b/new_vision_filter/include/kalman/UnscentedKalmanFilter.hpp new file mode 100644 index 000000000..cd13a345a --- /dev/null +++ b/new_vision_filter/include/kalman/UnscentedKalmanFilter.hpp @@ -0,0 +1,275 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_UNSCENTEDKALMANFILTER_HPP_ +#define KALMAN_UNSCENTEDKALMANFILTER_HPP_ + +#include "UnscentedKalmanFilterBase.hpp" +#include "StandardFilterBase.hpp" + +namespace Kalman { + + /** + * @brief Unscented Kalman Filter (UKF) + * + * @note It is recommended to use the square-root implementation SquareRootUnscentedKalmanFilter of this filter + * + * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. + * Whenever "the paper" is referenced within this file then this paper is meant. + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class UnscentedKalmanFilter : public UnscentedKalmanFilterBase, + public StandardFilterBase + { + public: + //! Unscented Kalman Filter base type + typedef UnscentedKalmanFilterBase UnscentedBase; + + //! Standard Filter base type + typedef StandardFilterBase StandardBase; + + //! Numeric Scalar Type inherited from base + using typename UnscentedBase::T; + + //! State Type inherited from base + using typename UnscentedBase::State; + + //! Measurement Model Type + template class CovarianceBase> + using MeasurementModelType = typename UnscentedBase::template MeasurementModelType; + + //! System Model Type + template class CovarianceBase> + using SystemModelType = typename UnscentedBase::template SystemModelType; + + protected: + //! The number of sigma points (depending on state dimensionality) + using UnscentedBase::SigmaPointCount; + + //! Matrix type containing the sigma state or measurement points + template + using SigmaPoints = typename UnscentedBase::template SigmaPoints; + + //! Kalman Gain Matrix Type + template + using KalmanGain = Kalman::KalmanGain; + + protected: + // Member variables + + //! State Estimate + using UnscentedBase::x; + + //! State Covariance + using StandardBase::P; + + //! Sigma points (state) + using UnscentedBase::sigmaStatePoints; + + public: + /** + * Constructor + * + * See paper for detailed parameter explanation + * + * @param alpha Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) + * @param beta Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) + * @param kappa Secondary scaling parameter (usually 0) + */ + UnscentedKalmanFilter(T alpha = T(1), T beta = T(2), T kappa = T(0)) + : UnscentedKalmanFilterBase(alpha, beta, kappa) + { + // Init covariance to identity + P.setIdentity(); + } + + /** + * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) + * + * @param [in] s The System model + * @return The updated state estimate + */ + template class CovarianceBase> + const State& predict( const SystemModelType& s ) + { + // predict state (without control) + Control u; + u.setZero(); + return predict( s, u ); + } + + /** + * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model + * + * @param [in] s The System model + * @param [in] u The Control input vector + * @return The updated state estimate + */ + template class CovarianceBase> + const State& predict( const SystemModelType& s, const Control& u ) + { + // Compute sigma points + if(!computeSigmaPoints()) + { + // TODO: handle numerical error + assert(false); + } + + // Compute predicted state + x = this->template computeStatePrediction(s, u); + + // Compute predicted covariance + computeCovarianceFromSigmaPoints(x, sigmaStatePoints, s.getCovariance(), P); + + // Return predicted state + return this->getState(); + } + + /** + * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model + * + * @param [in] m The Measurement model + * @param [in] z The measurement vector + * @return The updated state estimate + */ + template class CovarianceBase> + const State& update( const MeasurementModelType& m, const Measurement& z ) + { + SigmaPoints sigmaMeasurementPoints; + + // Predict measurement (and corresponding sigma points) + Measurement y = this->template computeMeasurementPrediction(m, sigmaMeasurementPoints); + + // Compute innovation covariance + Covariance P_yy; + computeCovarianceFromSigmaPoints(y, sigmaMeasurementPoints, m.getCovariance(), P_yy); + + KalmanGain K; + computeKalmanGain(y, sigmaMeasurementPoints, P_yy, K); + + // Update state + x += K * ( z - y ); + + // Update state covariance + updateStateCovariance(K, P_yy); + + return this->getState(); + } + + protected: + /** + * @brief Compute sigma points from current state estimate and state covariance + * + * @note This covers equations (6) and (9) of Algorithm 2.1 in the Paper + */ + bool computeSigmaPoints() + { + // Get square root of covariance + CovarianceSquareRoot llt; + llt.compute(P); + if(llt.info() != Eigen::Success) + { + return false; + } + + SquareMatrix _S = llt.matrixL().toDenseMatrix(); + + // Set left "block" (first column) + sigmaStatePoints.template leftCols<1>() = x; + // Set center block with x + gamma * S + sigmaStatePoints.template block(0,1) + = ( this->gamma * _S).colwise() + x; + // Set right block with x - gamma * S + sigmaStatePoints.template rightCols() + = (-this->gamma * _S).colwise() + x; + + return true; + } + + /** + * @brief Compute the Covariance from sigma points and noise covariance + * + * @param [in] mean The mean predicted state or measurement + * @param [in] sigmaPoints the predicted sigma state or measurement points + * @param [in] noiseCov The system or measurement noise covariance + * @param [out] cov The propagated state or innovation covariance + * + * @return True on success, false if a numerical error is encountered when updating the matrix + */ + template + bool computeCovarianceFromSigmaPoints( const Type& mean, const SigmaPoints& sigmaPoints, + const Covariance& noiseCov, Covariance& cov) + { + decltype(sigmaPoints) W = this->sigmaWeights_c.transpose().template replicate(); + decltype(sigmaPoints) tmp = (sigmaPoints.colwise() - mean); + cov = tmp.cwiseProduct(W) * tmp.transpose() + noiseCov; + + return true; + } + + /** + * @brief Compute the Kalman Gain from predicted measurement and sigma points and the innovation covariance. + * + * @note This covers equations (11) and (12) of Algorithm 2.1 in the Paper + * + * @param [in] y The predicted measurement + * @param [in] sigmaMeasurementPoints The predicted sigma measurement points + * @param [in] P_yy The innovation covariance + * @param [out] K The computed Kalman Gain matrix \f$ K \f$ + */ + template + bool computeKalmanGain( const Measurement& y, + const SigmaPoints& sigmaMeasurementPoints, + const Covariance& P_yy, + KalmanGain& K) + { + // Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs + // when Measurement::RowsAtCompileTime == 1 AND State::RowsAtCompileTime >= 8 + decltype(sigmaStatePoints) W = this->sigmaWeights_c.transpose().template replicate(); + Matrix P_xy + = (sigmaStatePoints.colwise() - x).cwiseProduct( W ).eval() + * (sigmaMeasurementPoints.colwise() - y).transpose(); + + K = P_xy * P_yy.inverse(); + return true; + } + + /** + * @brief Update the state covariance matrix using the Kalman Gain and the Innovation Covariance + * + * @note This covers equation (14) of Algorithm 2.1 in the Paper + * + * @param [in] K The computed Kalman Gain matrix + * @param [in] P_yy The innovation covariance + * @return True on success, false if a numerical error is encountered when updating the matrix + */ + template + bool updateStateCovariance(const KalmanGain& K, const Covariance& P_yy) + { + P -= K * P_yy * K.transpose(); + return true; + } + }; +} + +#endif diff --git a/new_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp b/new_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp new file mode 100644 index 000000000..25e7fb2e9 --- /dev/null +++ b/new_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp @@ -0,0 +1,232 @@ +// The MIT License (MIT) +// +// Copyright (c) 2015 Markus Herb +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. +#ifndef KALMAN_UNSCENTEDKALMANFILTERBASE_HPP_ +#define KALMAN_UNSCENTEDKALMANFILTERBASE_HPP_ + +#include + +#include "KalmanFilterBase.hpp" +#include "SystemModel.hpp" +#include "MeasurementModel.hpp" + +namespace Kalman { + + /** + * @brief Abstract Base for Unscented Kalman Filters + * + * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. + * Whenever "the paper" is referenced within this file then this paper is meant. + * + * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) + */ + template + class UnscentedKalmanFilterBase : public KalmanFilterBase + { + public: + // Public typedefs + //! Kalman Filter base type + typedef KalmanFilterBase Base; + + //! Numeric Scalar Type inherited from base + using typename Base::T; + + //! State Type inherited from base + using typename Base::State; + + //! Measurement Model Type + template class CovarianceBase> + using MeasurementModelType = MeasurementModel; + + //! System Model Type + template class CovarianceBase> + using SystemModelType = SystemModel; + + protected: + // Protected typedefs + //! The number of sigma points (depending on state dimensionality) + static constexpr int SigmaPointCount = 2 * State::RowsAtCompileTime + 1; + + //! Vector containg the sigma scaling weights + typedef Vector SigmaWeights; + + //! Matrix type containing the sigma state or measurement points + template + using SigmaPoints = Matrix; + + protected: + // Member variables + + //! State Estimate + using Base::x; + + //! Sigma weights (m) + SigmaWeights sigmaWeights_m; + //! Sigma weights (c) + SigmaWeights sigmaWeights_c; + + //! Sigma points (state) + SigmaPoints sigmaStatePoints; + + // Weight parameters + T alpha; //!< Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) + T beta; //!< Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) + T kappa; //!< Secondary scaling parameter (usually 0) + T gamma; //!< \f$ \gamma = \sqrt{L + \lambda} \f$ with \f$ L \f$ being the state dimensionality + T lambda; //!< \f$ \lambda = \alpha^2 ( L + \kappa ) - L\f$ with \f$ L \f$ being the state dimensionality + + protected: + /** + * Constructor + * + * See paper for parameter explanation + * + * @param _alpha Scaling parameter for spread of sigma points (usually 1e-4 <= alpha <= 1) + * @param _beta Parameter for prior knowledge about the distribution (beta = 2 is optimal for Gaussian) + * @param _kappa Secondary scaling parameter (usually 0) + */ + UnscentedKalmanFilterBase(T _alpha = T(1), T _beta = T(2), T _kappa = T(0)) : alpha(_alpha), beta(_beta), kappa(_kappa) + { + // Pre-compute all weights + computeWeights(); + + // Setup state and covariance + x.setZero(); + } + + /** + * @brief Compute predicted state using system model and control input + * + * @param [in] s The System Model + * @param [in] u The Control input + * @return The predicted state + */ + template class CovarianceBase> + State computeStatePrediction(const SystemModelType& s, const Control& u) + { + // Pass each sigma point through non-linear state transition function + computeSigmaPointTransition(s, u); + + // Compute predicted state from predicted sigma points + return computePredictionFromSigmaPoints(sigmaStatePoints); + } + + /** + * @brief Compute predicted measurement using measurement model and predicted sigma measurements + * + * @param [in] m The Measurement Model + * @param [in] sigmaMeasurementPoints The predicted sigma measurement points + * @return The predicted measurement + */ + template class CovarianceBase> + Measurement computeMeasurementPrediction(const MeasurementModelType& m, SigmaPoints& sigmaMeasurementPoints) + { + // Predict measurements for each sigma point + computeSigmaPointMeasurements(m, sigmaMeasurementPoints); + + // Predict measurement from sigma measurement points + return computePredictionFromSigmaPoints(sigmaMeasurementPoints); + } + + /** + * @brief Compute sigma weights + */ + void computeWeights() + { + T L = T(State::RowsAtCompileTime); + lambda = alpha * alpha * ( L + kappa ) - L; + gamma = std::sqrt( L + lambda ); + + // Make sure L != -lambda to avoid division by zero + assert( std::abs(L + lambda) > 1e-6 ); + + // Make sure L != -kappa to avoid division by zero + assert( std::abs(L + kappa) > 1e-6 ); + + T W_m_0 = lambda / ( L + lambda ); + T W_c_0 = W_m_0 + (T(1) - alpha*alpha + beta); + T W_i = T(1) / ( T(2) * alpha*alpha * (L + kappa) ); + + // Make sure W_i > 0 to avoid square-root of negative number + assert( W_i > T(0) ); + + sigmaWeights_m[0] = W_m_0; + sigmaWeights_c[0] = W_c_0; + + for(int i = 1; i < SigmaPointCount; ++i) + { + sigmaWeights_m[i] = W_i; + sigmaWeights_c[i] = W_i; + } + } + + /** + * @brief Predict expected sigma states from current sigma states using system model and control input + * + * @note This covers equation (18) of Algorithm 3.1 in the Paper + * + * @param [in] s The System Model + * @param [in] u The Control input + */ + template class CovarianceBase> + void computeSigmaPointTransition(const SystemModelType& s, const Control& u) + { + for( int i = 0; i < SigmaPointCount; ++i ) + { + sigmaStatePoints.col(i) = s.f( sigmaStatePoints.col(i), u ); + } + } + + /** + * @brief Predict the expected sigma measurements from predicted sigma states using measurement model + * + * @note This covers equation (23) of Algorithm 3.1 in the Paper + * + * @param [in] m The Measurement model + * @param [out] sigmaMeasurementPoints The struct of expected sigma measurements to be computed + */ + template class CovarianceBase> + void computeSigmaPointMeasurements(const MeasurementModelType& m, SigmaPoints& sigmaMeasurementPoints) + { + for( int i = 0; i < SigmaPointCount; ++i ) + { + sigmaMeasurementPoints.col(i) = m.h( sigmaStatePoints.col(i) ); + } + } + + /** + * @brief Compute state or measurement prediciton from sigma points using pre-computed sigma weights + * + * @note This covers equations (19) and (24) of Algorithm 3.1 in the Paper + * + * @param [in] sigmaPoints The computed sigma points of the desired type (state or measurement) + * @return The prediction + */ + template + Type computePredictionFromSigmaPoints(const SigmaPoints& sigmaPoints) + { + // Use efficient matrix x vector computation + return sigmaPoints * sigmaWeights_m; + } + }; +} + +#endif diff --git a/new_vision_filter/package.xml b/new_vision_filter/package.xml new file mode 100644 index 000000000..36299c4e1 --- /dev/null +++ b/new_vision_filter/package.xml @@ -0,0 +1,21 @@ + + + + new_vision_filter + 0.0.0 + TODO: Package description + christian + Apache-2.0 + + ament_cmake + + rclcpp + eigen + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/new_vision_filter/src/camera.cpp b/new_vision_filter/src/camera.cpp new file mode 100644 index 000000000..6862f3c1d --- /dev/null +++ b/new_vision_filter/src/camera.cpp @@ -0,0 +1,25 @@ +// Copyright 2025 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +class Camera { + // Need to process an individual frame + // Set geometry from VisionGeometryCameraCalibration.msg + // Have a queue/buffer that we can remove old frames/have a set capacity +} \ No newline at end of file diff --git a/new_vision_filter/src/camera.hpp b/new_vision_filter/src/camera.hpp new file mode 100644 index 000000000..e69de29bb diff --git a/new_vision_filter/src/kick/basic_chip.hpp b/new_vision_filter/src/kick/basic_chip.hpp new file mode 100644 index 000000000..f5a2315f3 --- /dev/null +++ b/new_vision_filter/src/kick/basic_chip.hpp @@ -0,0 +1 @@ +// Some sort of basic (non?)linear model to predict the expected trajectory of a chip that leaves the ground \ No newline at end of file diff --git a/new_vision_filter/src/kick/basic_kick.hpp b/new_vision_filter/src/kick/basic_kick.hpp new file mode 100644 index 000000000..28c26b811 --- /dev/null +++ b/new_vision_filter/src/kick/basic_kick.hpp @@ -0,0 +1 @@ +// Some sort of basic linear model to predict the expected trajectory of a kick on the ground \ No newline at end of file diff --git a/new_vision_filter/src/tracks/ball_track.hpp b/new_vision_filter/src/tracks/ball_track.hpp new file mode 100644 index 000000000..e69de29bb diff --git a/new_vision_filter/src/tracks/robot_track.hpp b/new_vision_filter/src/tracks/robot_track.hpp new file mode 100644 index 000000000..e69de29bb diff --git a/new_vision_filter/src/vision_filter_node.cpp b/new_vision_filter/src/vision_filter_node.cpp new file mode 100644 index 000000000..a6f3e11b9 --- /dev/null +++ b/new_vision_filter/src/vision_filter_node.cpp @@ -0,0 +1,35 @@ +// Copyright 2025 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include + +class VisionFilterNode : public rclcpp::Node +{ + // Subscribe to vision info from our processed messages + // in ssl_vision_bridge_node.cpp + + // Important things this needs to do, which could be broken out into objects that this holds + // and uses when the node is ticked... + + // Check quality of stuff overall + // Keep track of individual cameras - can be 1 - many + // Fuse the info from those cameras into tracks + // Output final prediction for robots and ball(s) +} \ No newline at end of file From d0754643aac3e00a5f4ecffa7f46e649e750bf14 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 2 Sep 2025 22:45:37 -0400 Subject: [PATCH 02/35] Add some basic things to vision filter node --- .../src/detect/kick_detector.hpp | 1 + new_vision_filter/src/vision_filter_node.cpp | 29 +++++++++++++++++-- 2 files changed, 27 insertions(+), 3 deletions(-) create mode 100644 new_vision_filter/src/detect/kick_detector.hpp diff --git a/new_vision_filter/src/detect/kick_detector.hpp b/new_vision_filter/src/detect/kick_detector.hpp new file mode 100644 index 000000000..619d1a2b2 --- /dev/null +++ b/new_vision_filter/src/detect/kick_detector.hpp @@ -0,0 +1 @@ +// Detects kicks and chips diff --git a/new_vision_filter/src/vision_filter_node.cpp b/new_vision_filter/src/vision_filter_node.cpp index a6f3e11b9..92f514c7e 100644 --- a/new_vision_filter/src/vision_filter_node.cpp +++ b/new_vision_filter/src/vision_filter_node.cpp @@ -20,11 +20,10 @@ #include +#include "camera.hpp" + class VisionFilterNode : public rclcpp::Node { - // Subscribe to vision info from our processed messages - // in ssl_vision_bridge_node.cpp - // Important things this needs to do, which could be broken out into objects that this holds // and uses when the node is ticked... @@ -32,4 +31,28 @@ class VisionFilterNode : public rclcpp::Node // Keep track of individual cameras - can be 1 - many // Fuse the info from those cameras into tracks // Output final prediction for robots and ball(s) + + public: + // Subscribe to vision info from our processed messages + // in ssl_vision_bridge_node.cpp + ssl_vision_subscription_ = + create_subscription( + std::string(Topics::kVisionMessages), + 10, + std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1)); + + // Will also need to add publishers here... but want to determine whether we keep existing + // approach that is in the old Vision Filter or do something else. + + private: + std::map cameras; + + void vision_callback(const ssl_league_msgs::msg::VisionWrapper vision_wrapper_msg) { + // Add vision data to the queue for each camera + } + + void timer_callback() { + // Publish the updated vision info + // Similar to the publish() method in TIGERs + } } \ No newline at end of file From 45bb90447753335445a3523b36073ff2192af459 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 14 Sep 2025 21:42:31 -0400 Subject: [PATCH 03/35] Added filtered ball and robot classes --- new_vision_filter/src/camera.cpp | 19 ++++++++++++++++++ new_vision_filter/src/filtered_ball.hpp | 20 +++++++++++++++++++ new_vision_filter/src/filtered_robot.hpp | 17 ++++++++++++++++ .../src/{detect => kick}/kick_detector.hpp | 0 .../ball_tracker.hpp} | 0 .../robot_tracker.hpp} | 0 6 files changed, 56 insertions(+) create mode 100644 new_vision_filter/src/filtered_ball.hpp create mode 100644 new_vision_filter/src/filtered_robot.hpp rename new_vision_filter/src/{detect => kick}/kick_detector.hpp (100%) rename new_vision_filter/src/{tracks/ball_track.hpp => tracker/ball_tracker.hpp} (100%) rename new_vision_filter/src/{tracks/robot_track.hpp => tracker/robot_tracker.hpp} (100%) diff --git a/new_vision_filter/src/camera.cpp b/new_vision_filter/src/camera.cpp index 6862f3c1d..539f777fd 100644 --- a/new_vision_filter/src/camera.cpp +++ b/new_vision_filter/src/camera.cpp @@ -17,9 +17,28 @@ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#include +#include class Camera { // Need to process an individual frame // Set geometry from VisionGeometryCameraCalibration.msg // Have a queue/buffer that we can remove old frames/have a set capacity + void process_detection_frame(){} + + void process_camera_geometry(){} + + void clear_old_messages(){} + + void process_balls(){} + + void process_robots(){} + + void create_new_ball_track(){} + + void create_new_robot_track(){} + + private: + std::deque detection_queue; + std::deque geometry_queue; } \ No newline at end of file diff --git a/new_vision_filter/src/filtered_ball.hpp b/new_vision_filter/src/filtered_ball.hpp new file mode 100644 index 000000000..34399dfb0 --- /dev/null +++ b/new_vision_filter/src/filtered_ball.hpp @@ -0,0 +1,20 @@ +#include +#include + +enum KickState { + ROLLING, + KICKED, + CHIPPED +}; + +class FilteredBall { + private: + KickState currentKickState= ROLLING; + std::chrono::time_point timestamp; + std::chrono::time_point last_visible_timestamp; + // TODO: Add unit labels/info + Eigen::Vector3 pos; + Eigen::Vector3 vel; + Eigen::Vector3 acc; + Eigen::Vector2 spin; +} \ No newline at end of file diff --git a/new_vision_filter/src/filtered_robot.hpp b/new_vision_filter/src/filtered_robot.hpp new file mode 100644 index 000000000..d5c3dc87d --- /dev/null +++ b/new_vision_filter/src/filtered_robot.hpp @@ -0,0 +1,17 @@ +#include +#include + +class FilteredRobot { + private: + int bot_id; + // TODO: use a global enum for yellow/blue? + int team; // for now 0 = us, 1 = not us + std::chrono::time_point timestamp; + std::chrono::time_point last_visible_timestamp; + // TODO: what units to use? + Eigen::Vector3 pos; + Eigen::Vector3 vel; + Eigen::Vector3 acc; + double orientation; // rads + double angularVel; // rads/sec +} \ No newline at end of file diff --git a/new_vision_filter/src/detect/kick_detector.hpp b/new_vision_filter/src/kick/kick_detector.hpp similarity index 100% rename from new_vision_filter/src/detect/kick_detector.hpp rename to new_vision_filter/src/kick/kick_detector.hpp diff --git a/new_vision_filter/src/tracks/ball_track.hpp b/new_vision_filter/src/tracker/ball_tracker.hpp similarity index 100% rename from new_vision_filter/src/tracks/ball_track.hpp rename to new_vision_filter/src/tracker/ball_tracker.hpp diff --git a/new_vision_filter/src/tracks/robot_track.hpp b/new_vision_filter/src/tracker/robot_tracker.hpp similarity index 100% rename from new_vision_filter/src/tracks/robot_track.hpp rename to new_vision_filter/src/tracker/robot_tracker.hpp From 6ea51bac9d011172bee29a9625f791aac3a57829 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 14 Sep 2025 22:24:33 -0400 Subject: [PATCH 04/35] Make a camera when there isn't one yet --- new_vision_filter/src/camera.cpp | 25 ++------------- new_vision_filter/src/camera.hpp | 32 ++++++++++++++++++++ new_vision_filter/src/vision_filter_node.cpp | 19 ++++++++++-- 3 files changed, 51 insertions(+), 25 deletions(-) diff --git a/new_vision_filter/src/camera.cpp b/new_vision_filter/src/camera.cpp index 539f777fd..0ef20fc8e 100644 --- a/new_vision_filter/src/camera.cpp +++ b/new_vision_filter/src/camera.cpp @@ -17,28 +17,7 @@ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include -#include -class Camera { - // Need to process an individual frame - // Set geometry from VisionGeometryCameraCalibration.msg - // Have a queue/buffer that we can remove old frames/have a set capacity - void process_detection_frame(){} +#include "camera.hpp" - void process_camera_geometry(){} - - void clear_old_messages(){} - - void process_balls(){} - - void process_robots(){} - - void create_new_ball_track(){} - - void create_new_robot_track(){} - - private: - std::deque detection_queue; - std::deque geometry_queue; -} \ No newline at end of file +Camera::Camera(int camera_id) : camera_id(camera_id) {}; \ No newline at end of file diff --git a/new_vision_filter/src/camera.hpp b/new_vision_filter/src/camera.hpp index e69de29bb..37f01a453 100644 --- a/new_vision_filter/src/camera.hpp +++ b/new_vision_filter/src/camera.hpp @@ -0,0 +1,32 @@ +#include +#include + +class Camera { + public: + // Need to process an individual frame + // Set geometry from VisionGeometryCameraCalibration.msg + // Have a queue/buffer that we can remove old frames/have a set capacity + Camera::Camera( + int camera_id + ); + + void process_detection_frame(); + + void process_camera_geometry(); + + void clear_old_messages(); + + void process_balls(); + + void process_robots(); + + void create_new_ball_track(); + + void create_new_robot_track(); + + std::deque detection_queue; + std::deque geometry_queue; + + private: + int camera_id; +} \ No newline at end of file diff --git a/new_vision_filter/src/vision_filter_node.cpp b/new_vision_filter/src/vision_filter_node.cpp index 92f514c7e..6569a5cab 100644 --- a/new_vision_filter/src/vision_filter_node.cpp +++ b/new_vision_filter/src/vision_filter_node.cpp @@ -47,8 +47,23 @@ class VisionFilterNode : public rclcpp::Node private: std::map cameras; - void vision_callback(const ssl_league_msgs::msg::VisionWrapper vision_wrapper_msg) { - // Add vision data to the queue for each camera + void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) { + // Add detections to the cameras' msg queues + auto detection = vision_wrapper_msg->detection; + // Create a new camera if we haven't seen this one before + int detect_camera = detection->camera_id; + if (!(cameras.contains(detect_camera))){ + cameras[detect_camera] = Camera(camera_id); + } + cameras[detect_camera].detection_queue.push_back(detection); + + // Add geometry to the cameras' msg queues + auto geometry = vision_wrapper_msg->geometry; + int geo_camera = geometry->camera_id; + if (!(cameras.contains(geo_camera))){ + cameras[geo_camera] = Camera(camera_id); + } + } void timer_callback() { From 09b924d655b930b11ab00ac6277cd665104b0801 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 21 Sep 2025 22:46:38 -0400 Subject: [PATCH 05/35] Start adding ball and robot processing, need to configure KF --- new_vision_filter/src/camera.cpp | 47 +++++++++++++++++++++++- new_vision_filter/src/camera.hpp | 22 +++++++---- new_vision_filter/src/filtered_ball.hpp | 12 ++++-- new_vision_filter/src/filtered_robot.hpp | 5 +++ new_vision_filter/src/frame_info.hpp | 6 +++ 5 files changed, 81 insertions(+), 11 deletions(-) create mode 100644 new_vision_filter/src/frame_info.hpp diff --git a/new_vision_filter/src/camera.cpp b/new_vision_filter/src/camera.cpp index 0ef20fc8e..981fc0bc5 100644 --- a/new_vision_filter/src/camera.cpp +++ b/new_vision_filter/src/camera.cpp @@ -19,5 +19,50 @@ // THE SOFTWARE. #include "camera.hpp" +#include "filtered_ball.hpp" +#include "filtered_robot.hpp" -Camera::Camera(int camera_id) : camera_id(camera_id) {}; \ No newline at end of file +Camera::Camera(int camera_id) : camera_id(camera_id) { + public: + // Need to process an individual frame + // Set geometry from VisionGeometryCameraCalibration.msg + // Have a queue/buffer that we can remove old frames/have a set capacity + + void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg){ + process_balls(detection_frame_msg); + process_robots(detection_frame_msg); + } + + void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData){}; + + void clear_old_messages(); + + void process_balls(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg){ + // Geometry msgs stuff... + // https://docs.ros2.org/foxy/api/geometry_msgs/index-msg.html + for (auto ball : detection_frame_msg->balls) { + // For all of our balls, see if we think this is close to an existing measurement + // If not, create a new one + tracked_balls.push_back(FilteredBall(ball)); + } + } + + void process_robots(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg){ + for (auto robot_detection : detection_frame_msg->robots){ + // Check if this is close to an existing measurement (from an id that we have?) + // Might want to refine that a bit since there are up to 24 robots ^ + // If not, create a new one + tracked_robots.push_back(FilteredRobot(robot_detection)); + } + } + + void create_new_ball_track(); + + void create_new_robot_track(); + + private: + int camera_id; + LastFrameInfo last_processed_frame; + std::vector tracked_balls; + std::vector tracked_robots; +} \ No newline at end of file diff --git a/new_vision_filter/src/camera.hpp b/new_vision_filter/src/camera.hpp index 37f01a453..01ffcf859 100644 --- a/new_vision_filter/src/camera.hpp +++ b/new_vision_filter/src/camera.hpp @@ -1,22 +1,27 @@ #include +#include #include +#include +#include +#include +#include "frame_info.hpp" +#include "filtered_ball.hpp" +#include "filtered_robot.hpp" class Camera { public: // Need to process an individual frame // Set geometry from VisionGeometryCameraCalibration.msg // Have a queue/buffer that we can remove old frames/have a set capacity - Camera::Camera( - int camera_id - ); + Camera::Camera(); - void process_detection_frame(); + void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg); - void process_camera_geometry(); + void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData); void clear_old_messages(); - void process_balls(); + void process_balls(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg); void process_robots(); @@ -27,6 +32,9 @@ class Camera { std::deque detection_queue; std::deque geometry_queue; - private: + private: int camera_id; + LastFrameInfo last_processed_frame; + std::vector tracked_balls; + std::vector tracked_robots; } \ No newline at end of file diff --git a/new_vision_filter/src/filtered_ball.hpp b/new_vision_filter/src/filtered_ball.hpp index 34399dfb0..e89222aa2 100644 --- a/new_vision_filter/src/filtered_ball.hpp +++ b/new_vision_filter/src/filtered_ball.hpp @@ -1,5 +1,7 @@ #include #include +#include +#include "kalman/ExtendedKalmanFilter.hpp" enum KickState { ROLLING, @@ -8,13 +10,17 @@ enum KickState { }; class FilteredBall { + FilteredBall::FilteredBall(ssl_league_msgs::msg::VisionDetectionBall vision_ball); + private: - KickState currentKickState= ROLLING; - std::chrono::time_point timestamp; - std::chrono::time_point last_visible_timestamp; + KickState currentKickState = ROLLING; + std::chrono::steady_clock::time_point timestamp; + std::chrono::steady_clock::time_point last_visible_timestamp; // TODO: Add unit labels/info Eigen::Vector3 pos; Eigen::Vector3 vel; Eigen::Vector3 acc; Eigen::Vector2 spin; + // Filter + ExtendedKalmanFilter posFilterXY; } \ No newline at end of file diff --git a/new_vision_filter/src/filtered_robot.hpp b/new_vision_filter/src/filtered_robot.hpp index d5c3dc87d..fb479864e 100644 --- a/new_vision_filter/src/filtered_robot.hpp +++ b/new_vision_filter/src/filtered_robot.hpp @@ -1,7 +1,10 @@ #include #include +#include class FilteredRobot { + FilteredRobot::FilteredRobot() + private: int bot_id; // TODO: use a global enum for yellow/blue? @@ -14,4 +17,6 @@ class FilteredRobot { Eigen::Vector3 acc; double orientation; // rads double angularVel; // rads/sec + // X, Y, Angle + ExtendedKalmanFilter posFilterXYW; } \ No newline at end of file diff --git a/new_vision_filter/src/frame_info.hpp b/new_vision_filter/src/frame_info.hpp new file mode 100644 index 000000000..b9407a8e4 --- /dev/null +++ b/new_vision_filter/src/frame_info.hpp @@ -0,0 +1,6 @@ +#include + +class LastFrameInfo { + public: + std::chrono::steady_clock::time_point time_processed; +} \ No newline at end of file From a1185bf55bc4425a5769ab33af515e3d2540733e Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 12 Oct 2025 23:59:40 -0400 Subject: [PATCH 06/35] Start doing some kalman filter stuff for the robot tracker --- .../CMakeLists.txt | 0 .../include/kalman/ExtendedKalmanFilter.hpp | 0 .../include/kalman/KalmanFilterBase.hpp | 0 .../kalman/LinearizedMeasurementModel.hpp | 0 .../include/kalman/LinearizedSystemModel.hpp | 0 .../include/kalman/Matrix.hpp | 0 .../include/kalman/MeasurementModel.hpp | 0 .../include/kalman/SquareRootBase.hpp | 0 .../kalman/SquareRootExtendedKalmanFilter.hpp | 0 .../include/kalman/SquareRootFilterBase.hpp | 0 .../kalman/SquareRootUnscentedKalmanFilter.hpp | 0 .../include/kalman/StandardBase.hpp | 0 .../include/kalman/StandardFilterBase.hpp | 0 .../include/kalman/SystemModel.hpp | 0 .../include/kalman/Types.hpp | 0 .../include/kalman/UnscentedKalmanFilter.hpp | 0 .../kalman/UnscentedKalmanFilterBase.hpp | 0 .../package.xml | 0 .../src/camera.cpp | 18 ++++++++++++++++-- .../src/camera.hpp | 0 .../src/filtered_ball.hpp | 0 new_super_vision_filter/src/filtered_robot.cpp | 14 ++++++++++++++ .../src/filtered_robot.hpp | 18 +++++++++++++++--- .../src/frame_info.hpp | 0 .../src/kick/basic_chip.hpp | 0 .../src/kick/basic_kick.hpp | 0 .../src/kick/kick_detector.hpp | 0 .../src/tracker/ball_tracker.hpp | 0 .../src/tracker/robot_tracker.hpp | 0 .../src/vision_filter_node.cpp | 0 30 files changed, 45 insertions(+), 5 deletions(-) rename {new_vision_filter => new_super_vision_filter}/CMakeLists.txt (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/ExtendedKalmanFilter.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/KalmanFilterBase.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/LinearizedMeasurementModel.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/LinearizedSystemModel.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/Matrix.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/MeasurementModel.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/SquareRootBase.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/SquareRootExtendedKalmanFilter.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/SquareRootFilterBase.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/SquareRootUnscentedKalmanFilter.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/StandardBase.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/StandardFilterBase.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/SystemModel.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/Types.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/UnscentedKalmanFilter.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/include/kalman/UnscentedKalmanFilterBase.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/package.xml (100%) rename {new_vision_filter => new_super_vision_filter}/src/camera.cpp (79%) rename {new_vision_filter => new_super_vision_filter}/src/camera.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/src/filtered_ball.hpp (100%) create mode 100644 new_super_vision_filter/src/filtered_robot.cpp rename {new_vision_filter => new_super_vision_filter}/src/filtered_robot.hpp (57%) rename {new_vision_filter => new_super_vision_filter}/src/frame_info.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/src/kick/basic_chip.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/src/kick/basic_kick.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/src/kick/kick_detector.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/src/tracker/ball_tracker.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/src/tracker/robot_tracker.hpp (100%) rename {new_vision_filter => new_super_vision_filter}/src/vision_filter_node.cpp (100%) diff --git a/new_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt similarity index 100% rename from new_vision_filter/CMakeLists.txt rename to new_super_vision_filter/CMakeLists.txt diff --git a/new_vision_filter/include/kalman/ExtendedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp similarity index 100% rename from new_vision_filter/include/kalman/ExtendedKalmanFilter.hpp rename to new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp diff --git a/new_vision_filter/include/kalman/KalmanFilterBase.hpp b/new_super_vision_filter/include/kalman/KalmanFilterBase.hpp similarity index 100% rename from new_vision_filter/include/kalman/KalmanFilterBase.hpp rename to new_super_vision_filter/include/kalman/KalmanFilterBase.hpp diff --git a/new_vision_filter/include/kalman/LinearizedMeasurementModel.hpp b/new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp similarity index 100% rename from new_vision_filter/include/kalman/LinearizedMeasurementModel.hpp rename to new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp diff --git a/new_vision_filter/include/kalman/LinearizedSystemModel.hpp b/new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp similarity index 100% rename from new_vision_filter/include/kalman/LinearizedSystemModel.hpp rename to new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp diff --git a/new_vision_filter/include/kalman/Matrix.hpp b/new_super_vision_filter/include/kalman/Matrix.hpp similarity index 100% rename from new_vision_filter/include/kalman/Matrix.hpp rename to new_super_vision_filter/include/kalman/Matrix.hpp diff --git a/new_vision_filter/include/kalman/MeasurementModel.hpp b/new_super_vision_filter/include/kalman/MeasurementModel.hpp similarity index 100% rename from new_vision_filter/include/kalman/MeasurementModel.hpp rename to new_super_vision_filter/include/kalman/MeasurementModel.hpp diff --git a/new_vision_filter/include/kalman/SquareRootBase.hpp b/new_super_vision_filter/include/kalman/SquareRootBase.hpp similarity index 100% rename from new_vision_filter/include/kalman/SquareRootBase.hpp rename to new_super_vision_filter/include/kalman/SquareRootBase.hpp diff --git a/new_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp similarity index 100% rename from new_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp rename to new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp diff --git a/new_vision_filter/include/kalman/SquareRootFilterBase.hpp b/new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp similarity index 100% rename from new_vision_filter/include/kalman/SquareRootFilterBase.hpp rename to new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp diff --git a/new_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp similarity index 100% rename from new_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp rename to new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp diff --git a/new_vision_filter/include/kalman/StandardBase.hpp b/new_super_vision_filter/include/kalman/StandardBase.hpp similarity index 100% rename from new_vision_filter/include/kalman/StandardBase.hpp rename to new_super_vision_filter/include/kalman/StandardBase.hpp diff --git a/new_vision_filter/include/kalman/StandardFilterBase.hpp b/new_super_vision_filter/include/kalman/StandardFilterBase.hpp similarity index 100% rename from new_vision_filter/include/kalman/StandardFilterBase.hpp rename to new_super_vision_filter/include/kalman/StandardFilterBase.hpp diff --git a/new_vision_filter/include/kalman/SystemModel.hpp b/new_super_vision_filter/include/kalman/SystemModel.hpp similarity index 100% rename from new_vision_filter/include/kalman/SystemModel.hpp rename to new_super_vision_filter/include/kalman/SystemModel.hpp diff --git a/new_vision_filter/include/kalman/Types.hpp b/new_super_vision_filter/include/kalman/Types.hpp similarity index 100% rename from new_vision_filter/include/kalman/Types.hpp rename to new_super_vision_filter/include/kalman/Types.hpp diff --git a/new_vision_filter/include/kalman/UnscentedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp similarity index 100% rename from new_vision_filter/include/kalman/UnscentedKalmanFilter.hpp rename to new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp diff --git a/new_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp b/new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp similarity index 100% rename from new_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp rename to new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp diff --git a/new_vision_filter/package.xml b/new_super_vision_filter/package.xml similarity index 100% rename from new_vision_filter/package.xml rename to new_super_vision_filter/package.xml diff --git a/new_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp similarity index 79% rename from new_vision_filter/src/camera.cpp rename to new_super_vision_filter/src/camera.cpp index 981fc0bc5..839abd6df 100644 --- a/new_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -18,6 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#include + #include "camera.hpp" #include "filtered_ball.hpp" #include "filtered_robot.hpp" @@ -48,11 +50,23 @@ Camera::Camera(int camera_id) : camera_id(camera_id) { } void process_robots(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg){ - for (auto robot_detection : detection_frame_msg->robots){ + for (auto robot_detection : detection_frame_msg->robots_blue){ + // Check if this is close to an existing measurement (from an id that we have?) + auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [](const FilteredRobot& bot) { + return bot.bot_id == robot_detection->robot_id; + }); + if (existing_bot != tracked_robots.end()){ + existing_bot.update(robot_detection); + } + // Might want to refine that a bit since there are up to 24 robots ^ + // If not, create a new one + tracked_robots.push_back(FilteredRobot(robot_detection, 0)); + } + for (auto robot_detection : detection_frame_msg->robots_yellow){ // Check if this is close to an existing measurement (from an id that we have?) // Might want to refine that a bit since there are up to 24 robots ^ // If not, create a new one - tracked_robots.push_back(FilteredRobot(robot_detection)); + tracked_robots.push_back(FilteredRobot(robot_detection, 1)); } } diff --git a/new_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp similarity index 100% rename from new_vision_filter/src/camera.hpp rename to new_super_vision_filter/src/camera.hpp diff --git a/new_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp similarity index 100% rename from new_vision_filter/src/filtered_ball.hpp rename to new_super_vision_filter/src/filtered_ball.hpp diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp new file mode 100644 index 000000000..1e2e34e80 --- /dev/null +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -0,0 +1,14 @@ +#include "filtered_robot.hpp" + +#include +#include + +FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, int team_color) + : posFilterXYW(Eigen::Vector3(robot_detection_msg->pose->position->x, robot_detection_msg->pose->position->y, robot_detection_msg->pose->orientation->w)), + bot_id(robot_detection_msg->robot_id), height(robot_detection_msg->height * 1000), team(team_color) {} + +void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg) { + // Make sure this detection isn't crazy off from our previous ones + // (unless our filter is still new/only has a few measurements) + // As long as its reasonable, update the Kalman Filter +} \ No newline at end of file diff --git a/new_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp similarity index 57% rename from new_vision_filter/src/filtered_robot.hpp rename to new_super_vision_filter/src/filtered_robot.hpp index fb479864e..21806580e 100644 --- a/new_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -1,22 +1,34 @@ +#ifndef FILTERED_ROBOT_HPP_ +#define FILTERED_ROBOT_HPP_ + #include #include #include class FilteredRobot { - FilteredRobot::FilteredRobot() + FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, int team); + + void update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection); private: int bot_id; // TODO: use a global enum for yellow/blue? int team; // for now 0 = us, 1 = not us + int age; + int oldEnough = 3; + int maxHealth = 20; + double maxDistance = -1.0; std::chrono::time_point timestamp; std::chrono::time_point last_visible_timestamp; - // TODO: what units to use? + // Unless otherwise stated, below is in m Eigen::Vector3 pos; Eigen::Vector3 vel; Eigen::Vector3 acc; double orientation; // rads double angularVel; // rads/sec + double height; // in m // X, Y, Angle ExtendedKalmanFilter posFilterXYW; -} \ No newline at end of file +} + +#endif // FILTERED_ROBOT_HPP_ \ No newline at end of file diff --git a/new_vision_filter/src/frame_info.hpp b/new_super_vision_filter/src/frame_info.hpp similarity index 100% rename from new_vision_filter/src/frame_info.hpp rename to new_super_vision_filter/src/frame_info.hpp diff --git a/new_vision_filter/src/kick/basic_chip.hpp b/new_super_vision_filter/src/kick/basic_chip.hpp similarity index 100% rename from new_vision_filter/src/kick/basic_chip.hpp rename to new_super_vision_filter/src/kick/basic_chip.hpp diff --git a/new_vision_filter/src/kick/basic_kick.hpp b/new_super_vision_filter/src/kick/basic_kick.hpp similarity index 100% rename from new_vision_filter/src/kick/basic_kick.hpp rename to new_super_vision_filter/src/kick/basic_kick.hpp diff --git a/new_vision_filter/src/kick/kick_detector.hpp b/new_super_vision_filter/src/kick/kick_detector.hpp similarity index 100% rename from new_vision_filter/src/kick/kick_detector.hpp rename to new_super_vision_filter/src/kick/kick_detector.hpp diff --git a/new_vision_filter/src/tracker/ball_tracker.hpp b/new_super_vision_filter/src/tracker/ball_tracker.hpp similarity index 100% rename from new_vision_filter/src/tracker/ball_tracker.hpp rename to new_super_vision_filter/src/tracker/ball_tracker.hpp diff --git a/new_vision_filter/src/tracker/robot_tracker.hpp b/new_super_vision_filter/src/tracker/robot_tracker.hpp similarity index 100% rename from new_vision_filter/src/tracker/robot_tracker.hpp rename to new_super_vision_filter/src/tracker/robot_tracker.hpp diff --git a/new_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp similarity index 100% rename from new_vision_filter/src/vision_filter_node.cpp rename to new_super_vision_filter/src/vision_filter_node.cpp From 452e65b003f2c51968e2dd287b09b2081280e337 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Mon, 10 Nov 2025 21:07:27 -0500 Subject: [PATCH 07/35] Small changes to filtered robot --- new_super_vision_filter/src/camera.cpp | 18 ++++++++++++------ new_super_vision_filter/src/filtered_robot.cpp | 7 +++++-- new_super_vision_filter/src/filtered_robot.hpp | 10 ++++++---- 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp index 839abd6df..2208db819 100644 --- a/new_super_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -57,16 +57,22 @@ Camera::Camera(int camera_id) : camera_id(camera_id) { }); if (existing_bot != tracked_robots.end()){ existing_bot.update(robot_detection); + } else { + // If not, create a new one + tracked_robots.push_back(FilteredRobot(robot_detection, 0)); } - // Might want to refine that a bit since there are up to 24 robots ^ - // If not, create a new one - tracked_robots.push_back(FilteredRobot(robot_detection, 0)); } for (auto robot_detection : detection_frame_msg->robots_yellow){ // Check if this is close to an existing measurement (from an id that we have?) - // Might want to refine that a bit since there are up to 24 robots ^ - // If not, create a new one - tracked_robots.push_back(FilteredRobot(robot_detection, 1)); + auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [](const FilteredRobot& bot) { + return bot.bot_id == robot_detection->robot_id; + }); + if (existing_bot != tracked_robots.end()){ + existing_bot.update(robot_detection); + } else { + // If not, create a new one + tracked_robots.push_back(FilteredRobot(robot_detection, 1)); + } } } diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 1e2e34e80..6b5a9f8ca 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -3,12 +3,15 @@ #include #include -FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, int team_color) +FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color) : posFilterXYW(Eigen::Vector3(robot_detection_msg->pose->position->x, robot_detection_msg->pose->position->y, robot_detection_msg->pose->orientation->w)), bot_id(robot_detection_msg->robot_id), height(robot_detection_msg->height * 1000), team(team_color) {} void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) + bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter -} \ No newline at end of file +} + +ateam_msgs::msg::RobotState FilteredRobot::toMsg(){}; \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 21806580e..ddd72c4ce 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -4,23 +4,25 @@ #include #include #include +#include class FilteredRobot { - FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, int team); + FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color); void update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection); + ateam_msgs::msg::RobotState toMsg(); + private: int bot_id; - // TODO: use a global enum for yellow/blue? - int team; // for now 0 = us, 1 = not us + ateam_common::TeamColor team; int age; int oldEnough = 3; int maxHealth = 20; double maxDistance = -1.0; std::chrono::time_point timestamp; std::chrono::time_point last_visible_timestamp; - // Unless otherwise stated, below is in m + // Unless otherwise stated, below is in m and s Eigen::Vector3 pos; Eigen::Vector3 vel; Eigen::Vector3 acc; From 31cf6a25d332d93170511a1e366e644353e0021d Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 16 Nov 2025 22:37:11 -0500 Subject: [PATCH 08/35] Add filter types and initialize state/covariance of robot kf --- new_super_vision_filter/src/filter_types.hpp | 142 ++++++++++++++++++ .../src/filtered_robot.cpp | 39 ++++- .../src/filtered_robot.hpp | 17 ++- 3 files changed, 188 insertions(+), 10 deletions(-) create mode 100644 new_super_vision_filter/src/filter_types.hpp diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp new file mode 100644 index 000000000..264a509f0 --- /dev/null +++ b/new_super_vision_filter/src/filter_types.hpp @@ -0,0 +1,142 @@ +#ifndef FILTER_TYPES_HPP_ +#define FILTER_TYPES_HPP_ + +#include "kalman/Types.hpp" +#include "kalman/LinearizedMeasurementModel.hpp" + +/* + Position measurement for robot or balls + + Structure is { + x_pos + y_pos + } + + Measurements are in m +*/ +class PosMeasurement : public Kalman::Vector +{ +public: + KALMAN_VECTOR(PosMeasurement, double, 2) +}; + +/* + 4D state vector for robot or ball position + + State vector structure is + { x_pos, + y_pos, + x_vel, + y_vel } + + Measurements are in m +*/ +class PosState : public Kalman::Vector +{ +public: + KALMAN_VECTOR(State, double, 4) + + static constexpr size_t PX = 0; + static constexpr size_t PY = 1; + static constexpr size_t VX = 2; + static constexpr size_t VY = 3; +}; + +/* + Measurement model for robots/balls' X and Y pos + velocity from + position measurements + + Curently assumes no measurement noise +*/ +class PosMeasurementModel + : public Kalman::LinearizedMeasurementModel +{ +public: + using MeasurementType = PosMeasurement; + using StateType = PosState; + + // h(x) = predicted measurement + MeasurementType h(const StateType& x) const override + { + PosMeasurement z; + z[0] = x[0]; // px + z[1] = x[1]; // py + return z; + } + + // Jacobian H = ∂h/∂x + void updateJacobians(const StateType& x) override + { + Jacobian H; + H.setZero(); + + H(0, 0) = 1; // dz_px / d_px + H(1, 1) = 1; // dz_py / d_py + + return H; + } +}; + +// TODO (Christian): Create SystemModel type similar to the above - based +// on LinearizedSystemModel + +/* + Angular position measurement (in rad) +*/ +class AngleMeasurement : public KalmanVector +{ +public: + KALMAN_VECTOR(AngleMeasurement, double, 1) +}; + +/* + Angular state (position and velocity) + + State vector format is + { + w_pos + w_vel + } +*/ +class AngleState : public Kalman::Vector +{ +public: + KALMAN_VECTOR(State, double, 2) + + static constexpr size_t PW = 0; + static constexpr size_t VW = 2; +}; + +/* + Linearized angle measurement model + + Currently assumes no measurement noise +*/ +class AngleMeasurementModel + : public Kalman::LinearizedMeasurementModel +{ +public: + using MeasurementType = AngleMeasurement; + using StateType = AngleState; + + // h(x) = predicted measurement + MeasurementType h(const StateType& x) const override + { + AngleMeasurement z; + z[0] = x[0]; // pw + return z; + } + + // Jacobian H = ∂h/∂x + void updateJacobians(const StateType& x) override + { + Jacobian H; + H.setZero(); + + H(0, 0) = 1; // dz_px / d_px + + return H; + } +}; + +#endif // FILTER_TYPES_HPP \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 6b5a9f8ca..c4c450b50 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -1,11 +1,46 @@ #include "filtered_robot.hpp" +// #include "kalman/Types.hpp" +#include "filter_types.hpp" #include #include FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color) - : posFilterXYW(Eigen::Vector3(robot_detection_msg->pose->position->x, robot_detection_msg->pose->position->y, robot_detection_msg->pose->orientation->w)), - bot_id(robot_detection_msg->robot_id), height(robot_detection_msg->height * 1000), team(team_color) {} + : posFilterXY(), posFilterW(), bot_id(robot_detection_msg->robot_id), height(robot_detection_msg->height * 1000), team(team_color) { + // TODO (Christian) - Might need to change the below to use our state/measurement types + // Initialize XY KF + Kalman::Vector<4, double> initial_state_xy = { + robot_detection_msg->pose->position->x, + robot_detection_msg->pose->position->y, + 0, + 0 + }; + posFilterXY.init(initial_state_xy); + Kalman::Matrix<4, 4, double> xy_covariance << 1e-2, 0, 0, 0, + 0, 1e-2, 0, 0, + 0, 0, 1e3, 0, + 0, 0, 0, 1e3; + posFilterXY.setCovariance(xy_covariance); + + // Initialize angular KF + /* + State vector is simply + w_pos, + w_vel + */ + Kalman::Vector<2, double> initial_state_w = { + robot_detection_msg->pose->orientation->w, + 0 + }; + posFilterW.init(initial_state_w); + /* + Initial covariance is approx 2 deg. for pos, + 10 deg. for vel + */ + Kalman::Matrix<2, 2, double> w_covariance << (2*M_PI/180.0)^2, 0, + 0, (10*M_PI/180.0)^2; + posFilterW.setCovariance(w_covariance); + } void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg) { // Make sure this detection isn't crazy off from our previous ones diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index ddd72c4ce..c02132b8a 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -1,6 +1,9 @@ #ifndef FILTERED_ROBOT_HPP_ #define FILTERED_ROBOT_HPP_ +#include "kalman/ExtendedKalmanFilter.hpp" +#include "filter_types.hpp" + #include #include #include @@ -22,15 +25,13 @@ class FilteredRobot { double maxDistance = -1.0; std::chrono::time_point timestamp; std::chrono::time_point last_visible_timestamp; - // Unless otherwise stated, below is in m and s - Eigen::Vector3 pos; - Eigen::Vector3 vel; - Eigen::Vector3 acc; - double orientation; // rads - double angularVel; // rads/sec double height; // in m - // X, Y, Angle - ExtendedKalmanFilter posFilterXYW; + // Use the below filtered values when getting X/Y/w (theta) and + // velocities + // X, Y + Kalman::ExtendedKalmanFilter posFilterXY; + // Theta + Kalman::ExtendedKalmanFilter posFilterW; } #endif // FILTERED_ROBOT_HPP_ \ No newline at end of file From d96edadc8ccb557d2cb04d1460854d3d24e03c5b Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 2 Dec 2025 22:30:23 -0500 Subject: [PATCH 09/35] Update cmake and packaging, fix vision filter types --- new_super_vision_filter/CMakeLists.txt | 28 +++++++++-- new_super_vision_filter/package.xml | 2 + new_super_vision_filter/src/filter_types.hpp | 47 ++++++++----------- .../src/vision_filter_node.cpp | 6 ++- 4 files changed, 52 insertions(+), 31 deletions(-) diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt index 12262f01e..d01f55f84 100644 --- a/new_super_vision_filter/CMakeLists.txt +++ b/new_super_vision_filter/CMakeLists.txt @@ -7,9 +7,31 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(ateam_msgs REQUIRED) +find_package(Eigen3 REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/vision_filter_node.cpp + src/camera.cpp + src/filtered_robot.cpp +) + +target_include_directories(${PROJECT_NAME} PRIVATE src include/kalman) +set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 20) +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_components + ateam_msgs + Eigen3 +) +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "new_super_vision::VisionFilterNode" + EXECUTABLE ateam_new_super_vision +) +install(TARGETS ${PROJECT_NAME} DESTINATION lib) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/new_super_vision_filter/package.xml b/new_super_vision_filter/package.xml index 36299c4e1..5c2054ef6 100644 --- a/new_super_vision_filter/package.xml +++ b/new_super_vision_filter/package.xml @@ -10,7 +10,9 @@ ament_cmake rclcpp + rclcpp_components eigen + ateam_msgs ament_lint_auto ament_lint_common diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp index 264a509f0..0852b0955 100644 --- a/new_super_vision_filter/src/filter_types.hpp +++ b/new_super_vision_filter/src/filter_types.hpp @@ -18,6 +18,14 @@ class PosMeasurement : public Kalman::Vector { public: KALMAN_VECTOR(PosMeasurement, double, 2) + + double x() const { + return (*this)[0]; + } + + double y() const { + return (*this)[1]; + } }; /* @@ -49,14 +57,11 @@ class PosState : public Kalman::Vector Curently assumes no measurement noise */ class PosMeasurementModel - : public Kalman::LinearizedMeasurementModel + : public Kalman::LinearizedMeasurementModel { public: - using MeasurementType = PosMeasurement; - using StateType = PosState; - // h(x) = predicted measurement - MeasurementType h(const StateType& x) const override + PosMeasurement h(const PosState& x) const { PosMeasurement z; z[0] = x[0]; // px @@ -65,21 +70,16 @@ class PosMeasurementModel } // Jacobian H = ∂h/∂x - void updateJacobians(const StateType& x) override +protected: + void updateJacobians(const PosState& x) { - Jacobian H; - H.setZero(); + this->H.setZero(); - H(0, 0) = 1; // dz_px / d_px - H(1, 1) = 1; // dz_py / d_py - - return H; + this->H(0, 0) = 1; // dz_px / d_px + this->H(1, 1) = 1; // dz_py / d_py } }; -// TODO (Christian): Create SystemModel type similar to the above - based -// on LinearizedSystemModel - /* Angular position measurement (in rad) */ @@ -104,7 +104,7 @@ class AngleState : public Kalman::Vector KALMAN_VECTOR(State, double, 2) static constexpr size_t PW = 0; - static constexpr size_t VW = 2; + static constexpr size_t VW = 1; }; /* @@ -113,14 +113,11 @@ class AngleState : public Kalman::Vector Currently assumes no measurement noise */ class AngleMeasurementModel - : public Kalman::LinearizedMeasurementModel + : public Kalman::LinearizedMeasurementModel { public: - using MeasurementType = AngleMeasurement; - using StateType = AngleState; - // h(x) = predicted measurement - MeasurementType h(const StateType& x) const override + AngleMeasurement h(const AngleState& x) const override { AngleMeasurement z; z[0] = x[0]; // pw @@ -130,12 +127,8 @@ class AngleMeasurementModel // Jacobian H = ∂h/∂x void updateJacobians(const StateType& x) override { - Jacobian H; - H.setZero(); - - H(0, 0) = 1; // dz_px / d_px - - return H; + this->H.setZero(); + this->H(0, 0) = 1; // dz_px / d_px } }; diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 6569a5cab..85d4284d7 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -22,6 +22,8 @@ #include "camera.hpp" +namespace new_super_vision { + class VisionFilterNode : public rclcpp::Node { // Important things this needs to do, which could be broken out into objects that this holds @@ -70,4 +72,6 @@ class VisionFilterNode : public rclcpp::Node // Publish the updated vision info // Similar to the publish() method in TIGERs } -} \ No newline at end of file +} + +} // namespace new_super_vision \ No newline at end of file From 342bc7dd7e860188eecd6c9cfa2688423440bd1a Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 16 Dec 2025 23:32:06 -0500 Subject: [PATCH 10/35] Add details to update step of filtered robot --- .../src/filtered_robot.cpp | 20 +++++++++++++++++++ .../src/filtered_robot.hpp | 2 ++ 2 files changed, 22 insertions(+) diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index c4c450b50..c0e105824 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -5,6 +5,10 @@ #include #include +// See https://thekalmanfilter.com/extended-kalman-filter-python-example/ +// or https://thekalmanfilter.com/kalman-filter-explained-simply/ +// OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp + FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color) : posFilterXY(), posFilterW(), bot_id(robot_detection_msg->robot_id), height(robot_detection_msg->height * 1000), team(team_color) { // TODO (Christian) - Might need to change the below to use our state/measurement types @@ -47,6 +51,22 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_dete // (unless our filter is still new/only has a few measurements) bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter + + // Predict state forward + // Predict covariance forward + // (All encompassed by the .predict() function) + auto xy_pred = posFilterXY.predict(); + auto w_pred = posFilterW.predict(); + // TODO - create the pos variable (should just be a vector) from our detection message + PosMeasurement xy_measurement = measurementModelXY.h(pos); + AngleMeasurement w_measurement = measurementModelW.h(pos); + // Update the Jacobian matrix (contained in filter) + // Compute Kalman gain (contained in filter) + // Update state estimate (returned) + // Update covariance estimate (contained in filter) + // All encompassed by the .update() function + auto xy_updated = posFilterXY.update(measurementModelXY, xy_measurement) + auto w_updated = posFilterW.update(measurementModelW, w_measurement) } ateam_msgs::msg::RobotState FilteredRobot::toMsg(){}; \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index c02132b8a..dc83be13d 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -30,8 +30,10 @@ class FilteredRobot { // velocities // X, Y Kalman::ExtendedKalmanFilter posFilterXY; + PosMeasurementModel measurementModelXY; // Theta Kalman::ExtendedKalmanFilter posFilterW; + AngleMeasurementModel measurementModelW; } #endif // FILTERED_ROBOT_HPP_ \ No newline at end of file From e2996045983c5a5c13c16154654b25659d41753e Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 20 Dec 2025 17:45:59 -0500 Subject: [PATCH 11/35] Update to filtered robot and create system models --- new_super_vision_filter/src/filter_types.hpp | 52 +++++++++++++++++++ .../src/filtered_robot.cpp | 9 ++++ .../src/filtered_robot.hpp | 5 +- 3 files changed, 64 insertions(+), 2 deletions(-) diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp index 0852b0955..d09b7a1c3 100644 --- a/new_super_vision_filter/src/filter_types.hpp +++ b/new_super_vision_filter/src/filter_types.hpp @@ -1,8 +1,11 @@ #ifndef FILTER_TYPES_HPP_ #define FILTER_TYPES_HPP_ +#include + #include "kalman/Types.hpp" #include "kalman/LinearizedMeasurementModel.hpp" +#include "kalman/LinearizedSystemModel.hpp" /* Position measurement for robot or balls @@ -80,6 +83,50 @@ class PosMeasurementModel } }; +/* + System (measurement to state) transition model for X/Y position + that assumes a constant velocity and no control inputs. + +*/ +class PosSystemModel : public Kalman::LinearizedSystemModel +{ + private: + const float ms_to_s = 1e4; + std::chrono::time_point last_update; + + public: + // For now, we don't add any control inputs to the vision system + // TODO (Christian): We might need this for the robots, even if we + // don't use it for the ball + using Control = Kalman::Vector; + + /* + The f() function applies what would be the A (state transition) + matrix but lazily skips doing a matmul. + + We assume the velocity stays constant and add v * dt to the + current position. + */ + PosState f(const PosState& x, const Control& /*u*/) const override + { + PosState x_updated; + const auto now = std::chrono::system_clock::now(); + std::chrono::milliseconds dt = now - last_update; + + // B/c dt is in ms, we need to convert to s, since + // our velocities are all in m/s + x_updated[x.PX] += x[x.VX] * dt * ms_to_s; + // We assume constant velocity in the system model + x_updated[x.VX] = x[x.VX]; + x_updated[x.PY] += x[x.VY] * dt * ms_to_s; + x_updated[x.VY] = x[x.VY]; + + last_update = now; + return last_updated; + } + +} + /* Angular position measurement (in rad) */ @@ -132,4 +179,9 @@ class AngleMeasurementModel } }; +class AngleSystemModel : public Kalman::LinearizedSystemModel +{ + +} + #endif // FILTER_TYPES_HPP \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index c0e105824..10c06bdb5 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -51,6 +51,13 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_dete // (unless our filter is still new/only has a few measurements) bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter + const std::chrono::time_point now = + std::chrono::system_clock::now(); + // If it's been too long, don't use this message + if (now - timestamp > update_threshold) { + timestamp = now; + return; + } // Predict state forward // Predict covariance forward @@ -67,6 +74,8 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_dete // All encompassed by the .update() function auto xy_updated = posFilterXY.update(measurementModelXY, xy_measurement) auto w_updated = posFilterW.update(measurementModelW, w_measurement) + // Update our timestamp for next time + timestamp = now; } ateam_msgs::msg::RobotState FilteredRobot::toMsg(){}; \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index dc83be13d..4383a20c3 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -23,8 +23,9 @@ class FilteredRobot { int oldEnough = 3; int maxHealth = 20; double maxDistance = -1.0; - std::chrono::time_point timestamp; - std::chrono::time_point last_visible_timestamp; + std::chrono::milliseconds update_threshold = 50; + std::chrono::time_point timestamp; + std::chrono::time_point last_visible_timestamp; double height; // in m // Use the below filtered values when getting X/Y/w (theta) and // velocities From 645625542883f2a57462217c50b0da7ce7db02ab Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 21 Dec 2025 21:04:45 -0500 Subject: [PATCH 12/35] Add filtered robot test outline, update filtered robot --- new_super_vision_filter/src/filter_types.hpp | 54 ++++++++++--- .../src/filtered_robot.cpp | 46 ++++++++--- .../src/filtered_robot.hpp | 23 +++++- new_super_vision_filter/test/CMakeLists.txt | 0 new_super_vision_filter/test/filter_test.cpp | 81 +++++++++++++++++++ 5 files changed, 183 insertions(+), 21 deletions(-) create mode 100644 new_super_vision_filter/test/CMakeLists.txt create mode 100644 new_super_vision_filter/test/filter_test.cpp diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp index d09b7a1c3..6bcc5b9e5 100644 --- a/new_super_vision_filter/src/filter_types.hpp +++ b/new_super_vision_filter/src/filter_types.hpp @@ -21,13 +21,16 @@ class PosMeasurement : public Kalman::Vector { public: KALMAN_VECTOR(PosMeasurement, double, 2) + + static constexpr size_t X = 0; + static constexpr size_t Y = 1; double x() const { - return (*this)[0]; + return (*this)[X]; } double y() const { - return (*this)[1]; + return (*this)[Y]; } }; @@ -51,6 +54,12 @@ class PosState : public Kalman::Vector static constexpr size_t PY = 1; static constexpr size_t VX = 2; static constexpr size_t VY = 3; + + double const px(){return (*this)[PX]; } + double const py(){return (*this)[PY]; } + double const vx(){return (*this)[VX]; } + double const vy(){return (*this)[VY]; } + }; /* @@ -67,8 +76,8 @@ class PosMeasurementModel PosMeasurement h(const PosState& x) const { PosMeasurement z; - z[0] = x[0]; // px - z[1] = x[1]; // py + z[0] = x.px(); // px + z[1] = x.py(); // py return z; } @@ -106,20 +115,23 @@ class PosSystemModel : public Kalman::LinearizedSystemModel We assume the velocity stays constant and add v * dt to the current position. + + pos_t = pos_{t-1} + vel_{t-1} * dt + vel_t = vel_{t-1} */ PosState f(const PosState& x, const Control& /*u*/) const override { - PosState x_updated; + PosState x_updated{}; const auto now = std::chrono::system_clock::now(); std::chrono::milliseconds dt = now - last_update; // B/c dt is in ms, we need to convert to s, since // our velocities are all in m/s - x_updated[x.PX] += x[x.VX] * dt * ms_to_s; + x_updated[x.PX] = x.px() + x.vx() * dt * ms_to_s; // We assume constant velocity in the system model - x_updated[x.VX] = x[x.VX]; - x_updated[x.PY] += x[x.VY] * dt * ms_to_s; - x_updated[x.VY] = x[x.VY]; + x_updated[x.VX] = x.vx(); + x_updated[x.PY] = x.py() + x.vy() * dt * ms_to_s; + x_updated[x.VY] = x.vy(); last_update = now; return last_updated; @@ -181,7 +193,31 @@ class AngleMeasurementModel class AngleSystemModel : public Kalman::LinearizedSystemModel { + private: + const float ms_to_s = 1e4; + std::chrono::time_point last_update; + + public: + /* + This is the system's state transition function (applies + the A matrix) + We assume the velocity stays constant and add v * dt to the + current position. + + pos_t = pos_{t-1} + vel_{t-1} * dt + vel_t = vel_{t-1} + */ + AngleState f(const AngleState& x, const Control& ){ + AngleState x_updated{}; + + const auto now = std::chrono::system_clock::now(); + std::chrono::milliseconds dt = now - last_update; + + x_updated[x.PW] = x.pw() + x.vw() * dt * ms_to_s; + + return x_updated; + } } #endif // FILTER_TYPES_HPP \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 10c06bdb5..2b3924610 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -1,5 +1,23 @@ +// Copyright 2024 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. #include "filtered_robot.hpp" -// #include "kalman/Types.hpp" #include "filter_types.hpp" #include @@ -13,7 +31,7 @@ FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_de : posFilterXY(), posFilterW(), bot_id(robot_detection_msg->robot_id), height(robot_detection_msg->height * 1000), team(team_color) { // TODO (Christian) - Might need to change the below to use our state/measurement types // Initialize XY KF - Kalman::Vector<4, double> initial_state_xy = { + PosState initial_state_xy = { robot_detection_msg->pose->position->x, robot_detection_msg->pose->position->y, 0, @@ -32,7 +50,7 @@ FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_de w_pos, w_vel */ - Kalman::Vector<2, double> initial_state_w = { + AngleState initial_state_w = { robot_detection_msg->pose->orientation->w, 0 }; @@ -49,33 +67,39 @@ FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_de void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) + ++age; bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter const std::chrono::time_point now = std::chrono::system_clock::now(); // If it's been too long, don't use this message - if (now - timestamp > update_threshold) { + if (now - timestamp > update_threshold || is_new) { timestamp = now; return; } - + // Update our timestamp for next time + timestamp = now; // Predict state forward // Predict covariance forward // (All encompassed by the .predict() function) auto xy_pred = posFilterXY.predict(); auto w_pred = posFilterW.predict(); - // TODO - create the pos variable (should just be a vector) from our detection message + PosMeasurement pos { + robot_detection_msg->pose->position->x, + robot_detection_msg->pose->position->y + }; + AngleMeasurement angle { + robot_detection_msg->pose->orientation->w + }; PosMeasurement xy_measurement = measurementModelXY.h(pos); - AngleMeasurement w_measurement = measurementModelW.h(pos); + AngleMeasurement w_measurement = measurementModelW.h(angle); // Update the Jacobian matrix (contained in filter) // Compute Kalman gain (contained in filter) // Update state estimate (returned) // Update covariance estimate (contained in filter) // All encompassed by the .update() function - auto xy_updated = posFilterXY.update(measurementModelXY, xy_measurement) - auto w_updated = posFilterW.update(measurementModelW, w_measurement) - // Update our timestamp for next time - timestamp = now; + posXYEstimate = posFilterXY.update(measurementModelXY, xy_measurement); + posWEstimate = w_updated = posFilterW.update(measurementModelW, w_measurement); } ateam_msgs::msg::RobotState FilteredRobot::toMsg(){}; \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 4383a20c3..58e7e2393 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -1,3 +1,22 @@ +// Copyright 2024 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. #ifndef FILTERED_ROBOT_HPP_ #define FILTERED_ROBOT_HPP_ @@ -19,7 +38,7 @@ class FilteredRobot { private: int bot_id; ateam_common::TeamColor team; - int age; + int age = 0; int oldEnough = 3; int maxHealth = 20; double maxDistance = -1.0; @@ -32,9 +51,11 @@ class FilteredRobot { // X, Y Kalman::ExtendedKalmanFilter posFilterXY; PosMeasurementModel measurementModelXY; + PosState posXYEstimate{}; // Theta Kalman::ExtendedKalmanFilter posFilterW; AngleMeasurementModel measurementModelW; + AngleState posWEstimate{}; } #endif // FILTERED_ROBOT_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/test/CMakeLists.txt b/new_super_vision_filter/test/CMakeLists.txt new file mode 100644 index 000000000..e69de29bb diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp new file mode 100644 index 000000000..1396f9a4b --- /dev/null +++ b/new_super_vision_filter/test/filter_test.cpp @@ -0,0 +1,81 @@ +// Copyright 2024 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include +#include + +#include +#include + +#include "filtered_robot.hpp" + +class FilteredRobotTest : public ::testing::Test +{ + protected: + void SetUp() override + { + ssl_league_msgs::msg::VisionDetectionRobot robot_msg{}; + auto team = ateam_common::TeamColor::Blue; + FilteredRobot robot{ + robot_msg, + team + }; + } +} + +TEST_F(RobotSetupTest, InitalState) +{ + // Check that initial state is valid + return; +} + +TEST_F(RobotSetupTest, InitialCovariance) +{ + // Check that initial covariance is valid + return; +} + +TEST_F(RobotUpdateTest, WaitUntilOldEnough) +{ + // Create valid timestamp and fake measurement + // Try to update + // Check that we didn't + // Do that again until "oldEnough" + // Now check that it has updated + return; +} + +TEST_F(RobotUpdateTest, UpdateIfTimestampValid) +{ + // Get initial estimate + // Create valid timestamp and fake measurement + // Update the filter + // Check that the current estimate is different + return; +} + +TEST_F(RobotUpdateTest, DontUpdateIfTimestampInvalid) +{ + // Get initial estimate + // Create invalid timestamp and fake measurement + // Attempt to update the filter + // Check that the current estimate is not different + return; +} \ No newline at end of file From 09918803269f97f2b5a7c546beef11ea99e59ff8 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 21 Dec 2025 22:21:28 -0500 Subject: [PATCH 13/35] Building but so many compiler errors --- new_super_vision_filter/CMakeLists.txt | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt index d01f55f84..7a61a0a18 100644 --- a/new_super_vision_filter/CMakeLists.txt +++ b/new_super_vision_filter/CMakeLists.txt @@ -10,6 +10,8 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(ateam_msgs REQUIRED) +find_package(ateam_common REQUIRED) +find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) add_library(${PROJECT_NAME} SHARED @@ -18,12 +20,19 @@ add_library(${PROJECT_NAME} SHARED src/filtered_robot.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE src include/kalman) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src +) set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 20) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components ateam_msgs + ateam_common Eigen3 ) rclcpp_components_register_node( @@ -31,7 +40,13 @@ rclcpp_components_register_node( PLUGIN "new_super_vision::VisionFilterNode" EXECUTABLE ateam_new_super_vision ) -install(TARGETS ${PROJECT_NAME} DESTINATION lib) +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -43,6 +58,7 @@ if(BUILD_TESTING) # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() + add_subdirectory(test) endif() ament_package() From b3696b0324973fca1f1ae6e1afac7dbfedc8c0db Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 3 Jan 2026 19:00:53 -0500 Subject: [PATCH 14/35] Fixed many compiler errors --- new_super_vision_filter/package.xml | 1 + new_super_vision_filter/src/camera.cpp | 6 +- new_super_vision_filter/src/camera.hpp | 33 ++++++++-- new_super_vision_filter/src/filter_types.hpp | 35 +++++----- new_super_vision_filter/src/filtered_ball.hpp | 9 ++- .../src/filtered_robot.cpp | 64 +++++++++++-------- .../src/filtered_robot.hpp | 11 ++-- .../src/vision_filter_node.cpp | 23 ++++--- new_super_vision_filter/test/CMakeLists.txt | 3 + new_super_vision_filter/test/filter_test.cpp | 1 - 10 files changed, 118 insertions(+), 68 deletions(-) diff --git a/new_super_vision_filter/package.xml b/new_super_vision_filter/package.xml index 5c2054ef6..82cdef06f 100644 --- a/new_super_vision_filter/package.xml +++ b/new_super_vision_filter/package.xml @@ -13,6 +13,7 @@ rclcpp_components eigen ateam_msgs + ateam_common ament_lint_auto ament_lint_common diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp index 2208db819..7ebf8484f 100644 --- a/new_super_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -76,13 +76,13 @@ Camera::Camera(int camera_id) : camera_id(camera_id) { } } - void create_new_ball_track(); + void create_new_ball_track(){} - void create_new_robot_track(); + void create_new_robot_track(){} private: int camera_id; LastFrameInfo last_processed_frame; std::vector tracked_balls; std::vector tracked_robots; -} \ No newline at end of file +}; diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp index 01ffcf859..3cafd5ac2 100644 --- a/new_super_vision_filter/src/camera.hpp +++ b/new_super_vision_filter/src/camera.hpp @@ -1,3 +1,26 @@ +// Copyright 2025 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef CAMERA_HPP_ +#define CAMERA_HPP_ + #include #include #include @@ -13,11 +36,11 @@ class Camera { // Need to process an individual frame // Set geometry from VisionGeometryCameraCalibration.msg // Have a queue/buffer that we can remove old frames/have a set capacity - Camera::Camera(); + Camera::Camera(int camera_id); void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg); - void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData); + void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData geometry); void clear_old_messages(); @@ -36,5 +59,7 @@ class Camera { int camera_id; LastFrameInfo last_processed_frame; std::vector tracked_balls; - std::vector tracked_robots; -} \ No newline at end of file + std::vector tracked_robots; +}; + +#endif // CAMERA_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp index 6bcc5b9e5..2cae7bd50 100644 --- a/new_super_vision_filter/src/filter_types.hpp +++ b/new_super_vision_filter/src/filter_types.hpp @@ -48,7 +48,7 @@ class PosMeasurement : public Kalman::Vector class PosState : public Kalman::Vector { public: - KALMAN_VECTOR(State, double, 4) + KALMAN_VECTOR(PosState, double, 4) static constexpr size_t PX = 0; static constexpr size_t PY = 1; @@ -123,21 +123,21 @@ class PosSystemModel : public Kalman::LinearizedSystemModel { PosState x_updated{}; const auto now = std::chrono::system_clock::now(); - std::chrono::milliseconds dt = now - last_update; - + std::chrono::duration dt = now - last_update; + // B/c dt is in ms, we need to convert to s, since // our velocities are all in m/s - x_updated[x.PX] = x.px() + x.vx() * dt * ms_to_s; + x_updated(x.PX) = x(x.PX) + x(x.VX) * dt.count() * ms_to_s; // We assume constant velocity in the system model - x_updated[x.VX] = x.vx(); - x_updated[x.PY] = x.py() + x.vy() * dt * ms_to_s; - x_updated[x.VY] = x.vy(); + x_updated(x.VX) = x(x.VX); + x_updated(x.PY) = x(x.PY) + x(x.VY) * dt.count() * ms_to_s; + x_updated(x.VY) = x(x.VY); last_update = now; - return last_updated; + return x_updated; } -} +}; /* Angular position measurement (in rad) @@ -160,10 +160,13 @@ class AngleMeasurement : public KalmanVector class AngleState : public Kalman::Vector { public: - KALMAN_VECTOR(State, double, 2) + KALMAN_VECTOR(AngleState, double, 2) static constexpr size_t PW = 0; static constexpr size_t VW = 1; + + double const pw(){return (*this)[PW]; } + double const vw(){return (*this)[VW]; } }; /* @@ -172,13 +175,13 @@ class AngleState : public Kalman::Vector Currently assumes no measurement noise */ class AngleMeasurementModel - : public Kalman::LinearizedMeasurementModel + : public Kalman::LinearizedMeasurementModel { public: // h(x) = predicted measurement AngleMeasurement h(const AngleState& x) const override { - AngleMeasurement z; + AngleMeasurement z{}; z[0] = x[0]; // pw return z; } @@ -208,16 +211,16 @@ class AngleSystemModel : public Kalman::LinearizedSystemModel pos_t = pos_{t-1} + vel_{t-1} * dt vel_t = vel_{t-1} */ - AngleState f(const AngleState& x, const Control& ){ + AngleState f(const AngleState& x, const Control& ) const { AngleState x_updated{}; const auto now = std::chrono::system_clock::now(); - std::chrono::milliseconds dt = now - last_update; + std::chrono::duration dt = now - last_update; - x_updated[x.PW] = x.pw() + x.vw() * dt * ms_to_s; + x_updated(x.PW) = x(x.PW) + x(x.VW) * dt.count() * ms_to_s; return x_updated; } -} +}; #endif // FILTER_TYPES_HPP \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp index e89222aa2..34b681d25 100644 --- a/new_super_vision_filter/src/filtered_ball.hpp +++ b/new_super_vision_filter/src/filtered_ball.hpp @@ -1,3 +1,6 @@ +#ifndef FILTERED_BALL_HPP_ +#define FILTERED_BALL_HPP_ + #include #include #include @@ -22,5 +25,7 @@ class FilteredBall { Eigen::Vector3 acc; Eigen::Vector2 spin; // Filter - ExtendedKalmanFilter posFilterXY; -} \ No newline at end of file + ExtendedKalmanFilter posFilterXY; +}; + +#endif // FILTERED_BALL_HPP_ diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 2b3924610..af893c5b1 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -20,6 +20,7 @@ #include "filtered_robot.hpp" #include "filter_types.hpp" +#include #include #include @@ -27,21 +28,22 @@ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ // OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp -FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color) +FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection_msg, ateam_common::TeamColor team_color) : posFilterXY(), posFilterW(), bot_id(robot_detection_msg->robot_id), height(robot_detection_msg->height * 1000), team(team_color) { // TODO (Christian) - Might need to change the below to use our state/measurement types // Initialize XY KF - PosState initial_state_xy = { - robot_detection_msg->pose->position->x, - robot_detection_msg->pose->position->y, + PosState initial_state_xy; + initial_state_xy << + robot_detection_msg->pose.position.x, + robot_detection_msg->pose.position.y, 0, - 0 - }; + 0; posFilterXY.init(initial_state_xy); - Kalman::Matrix<4, 4, double> xy_covariance << 1e-2, 0, 0, 0, - 0, 1e-2, 0, 0, - 0, 0, 1e3, 0, - 0, 0, 0, 1e3; + Kalman::Matrix xy_covariance; + xy_covariance << 1e-2, 0, 0, 0, + 0, 1e-2, 0, 0, + 0, 0, 1e3, 0, + 0, 0, 0, 1e3; posFilterXY.setCovariance(xy_covariance); // Initialize angular KF @@ -50,21 +52,22 @@ FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_de w_pos, w_vel */ - AngleState initial_state_w = { - robot_detection_msg->pose->orientation->w, - 0 - }; + AngleState initial_state_w; + initial_state_w << + robot_detection_msg->pose.orientation.w, + 0; posFilterW.init(initial_state_w); /* Initial covariance is approx 2 deg. for pos, 10 deg. for vel */ - Kalman::Matrix<2, 2, double> w_covariance << (2*M_PI/180.0)^2, 0, - 0, (10*M_PI/180.0)^2; + Kalman::Matrix w_covariance; + w_covariance << std::pow(2*M_PI/180.0, 2), 0, + 0, std::pow(10*M_PI/180.0, 2); posFilterW.setCovariance(w_covariance); } -void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg) { +void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection_msg) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) ++age; @@ -82,15 +85,18 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_dete // Predict state forward // Predict covariance forward // (All encompassed by the .predict() function) - auto xy_pred = posFilterXY.predict(); - auto w_pred = posFilterW.predict(); - PosMeasurement pos { - robot_detection_msg->pose->position->x, - robot_detection_msg->pose->position->y - }; - AngleMeasurement angle { - robot_detection_msg->pose->orientation->w - }; + auto xy_pred = posFilterXY.predict(systemModelXY); + auto w_pred = posFilterW.predict(systemModelW); + PosState pos; + pos << + robot_detection_msg->pose.position.x, + robot_detection_msg->pose.position.y, + 0, + 0; + AngleState angle; + angle << + robot_detection_msg->pose.orientation.w, + 0; PosMeasurement xy_measurement = measurementModelXY.h(pos); AngleMeasurement w_measurement = measurementModelW.h(angle); // Update the Jacobian matrix (contained in filter) @@ -99,7 +105,9 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_dete // Update covariance estimate (contained in filter) // All encompassed by the .update() function posXYEstimate = posFilterXY.update(measurementModelXY, xy_measurement); - posWEstimate = w_updated = posFilterW.update(measurementModelW, w_measurement); + posWEstimate = posFilterW.update(measurementModelW, w_measurement); } -ateam_msgs::msg::RobotState FilteredRobot::toMsg(){}; \ No newline at end of file +ateam_msgs::msg::RobotState FilteredRobot::toMsg(){ + return ateam_msgs::msg::RobotState{}; +}; \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 58e7e2393..85132629f 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -27,11 +27,12 @@ #include #include #include +#include class FilteredRobot { - FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color); + FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection_msg, ateam_common::TeamColor team_color); - void update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection); + void update(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection); ateam_msgs::msg::RobotState toMsg(); @@ -42,7 +43,7 @@ class FilteredRobot { int oldEnough = 3; int maxHealth = 20; double maxDistance = -1.0; - std::chrono::milliseconds update_threshold = 50; + std::chrono::milliseconds update_threshold{50}; std::chrono::time_point timestamp; std::chrono::time_point last_visible_timestamp; double height; // in m @@ -50,12 +51,14 @@ class FilteredRobot { // velocities // X, Y Kalman::ExtendedKalmanFilter posFilterXY; + PosSystemModel systemModelXY; PosMeasurementModel measurementModelXY; PosState posXYEstimate{}; // Theta Kalman::ExtendedKalmanFilter posFilterW; + AngleSystemModel systemModelW; AngleMeasurementModel measurementModelW; AngleState posWEstimate{}; -} +}; #endif // FILTERED_ROBOT_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 85d4284d7..e7a1c03ad 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -20,6 +20,8 @@ #include +#include + #include "camera.hpp" namespace new_super_vision { @@ -35,21 +37,21 @@ class VisionFilterNode : public rclcpp::Node // Output final prediction for robots and ball(s) public: - // Subscribe to vision info from our processed messages - // in ssl_vision_bridge_node.cpp - ssl_vision_subscription_ = - create_subscription( - std::string(Topics::kVisionMessages), - 10, - std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1)); // Will also need to add publishers here... but want to determine whether we keep existing // approach that is in the old Vision Filter or do something else. private: + // Subscribe to vision info from our processed messages + // in ssl_vision_bridge_node.cpp + rclcpp::Subscription::SharedPtr ssl_vision_subscription_ = + create_subscription( + std::string(Topics::kVisionMessages), + 10, + std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1)); std::map cameras; - void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) { + void vision_callback(const ssl_league_msgs::msg::vision::VisionWrapper::SharedPtr vision_wrapper_msg) { // Add detections to the cameras' msg queues auto detection = vision_wrapper_msg->detection; // Create a new camera if we haven't seen this one before @@ -65,13 +67,14 @@ class VisionFilterNode : public rclcpp::Node if (!(cameras.contains(geo_camera))){ cameras[geo_camera] = Camera(camera_id); } - + return; } void timer_callback() { // Publish the updated vision info // Similar to the publish() method in TIGERs + return; } -} +}; } // namespace new_super_vision \ No newline at end of file diff --git a/new_super_vision_filter/test/CMakeLists.txt b/new_super_vision_filter/test/CMakeLists.txt index e69de29bb..0edaf0359 100644 --- a/new_super_vision_filter/test/CMakeLists.txt +++ b/new_super_vision_filter/test/CMakeLists.txt @@ -0,0 +1,3 @@ +find_package(ament_cmake_gtest REQUIRED) +ament_add_gtest(filter_test filter_test.cpp) +target_link_libraries(filter_test ${PROJECT_NAME}) diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp index 1396f9a4b..1d9004acd 100644 --- a/new_super_vision_filter/test/filter_test.cpp +++ b/new_super_vision_filter/test/filter_test.cpp @@ -19,7 +19,6 @@ // THE SOFTWARE. #include -#include #include #include From 574f120493d0b9769c20eeaafe2b0e36a58f3418 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 3 Jan 2026 21:07:47 -0500 Subject: [PATCH 15/35] Fixing more errors --- new_super_vision_filter/src/camera.cpp | 101 ++++++++---------- new_super_vision_filter/src/camera.hpp | 10 +- new_super_vision_filter/src/filter_types.hpp | 24 ++--- new_super_vision_filter/src/filtered_ball.hpp | 4 +- .../src/filtered_robot.cpp | 6 +- .../src/filtered_robot.hpp | 4 +- new_super_vision_filter/src/frame_info.hpp | 7 +- .../src/vision_filter_node.cpp | 35 +++--- 8 files changed, 102 insertions(+), 89 deletions(-) diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp index 7ebf8484f..e66016450 100644 --- a/new_super_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -24,65 +24,58 @@ #include "filtered_ball.hpp" #include "filtered_robot.hpp" -Camera::Camera(int camera_id) : camera_id(camera_id) { - public: - // Need to process an individual frame - // Set geometry from VisionGeometryCameraCalibration.msg - // Have a queue/buffer that we can remove old frames/have a set capacity +Camera::Camera(int camera_id) : camera_id(camera_id) {}; +// Need to process an individual frame +// Set geometry from VisionGeometryCameraCalibration.msg +// Have a queue/buffer that we can remove old frames/have a set capacity - void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg){ - process_balls(detection_frame_msg); - process_robots(detection_frame_msg); - } +void Camera::process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg){ + process_balls(detection_frame_msg); + process_robots(detection_frame_msg); +} - void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData){}; +void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData::SharedPtr){}; - void clear_old_messages(); +void Camera::clear_old_messages(){}; - void process_balls(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg){ - // Geometry msgs stuff... - // https://docs.ros2.org/foxy/api/geometry_msgs/index-msg.html - for (auto ball : detection_frame_msg->balls) { - // For all of our balls, see if we think this is close to an existing measurement - // If not, create a new one - tracked_balls.push_back(FilteredBall(ball)); - } - } +void Camera::process_balls(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg){ + // Geometry msgs stuff... + // https://docs.ros2.org/foxy/api/geometry_msgs/index-msg.html + // for (auto ball : detection_frame_msg->balls) { + // // For all of our balls, see if we think this is close to an existing measurement + // // If not, create a new one + // tracked_balls.push_back(FilteredBall(ball)); + // } + return; +}; - void process_robots(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg){ - for (auto robot_detection : detection_frame_msg->robots_blue){ - // Check if this is close to an existing measurement (from an id that we have?) - auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [](const FilteredRobot& bot) { - return bot.bot_id == robot_detection->robot_id; - }); - if (existing_bot != tracked_robots.end()){ - existing_bot.update(robot_detection); - } else { - // If not, create a new one - tracked_robots.push_back(FilteredRobot(robot_detection, 0)); - } - } - for (auto robot_detection : detection_frame_msg->robots_yellow){ - // Check if this is close to an existing measurement (from an id that we have?) - auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [](const FilteredRobot& bot) { - return bot.bot_id == robot_detection->robot_id; - }); - if (existing_bot != tracked_robots.end()){ - existing_bot.update(robot_detection); - } else { - // If not, create a new one - tracked_robots.push_back(FilteredRobot(robot_detection, 1)); - } - } +void Camera::process_robots(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg){ + for (auto robot_detection : detection_frame_msg->robots_blue){ + // Check if this is close to an existing measurement (from an id that we have?) + auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [](const FilteredRobot& bot) { + return bot.getId() == robot_detection.robot_id; + }); + if (existing_bot != tracked_robots.end()){ + existing_bot.update(robot_detection); + } else { + // If not, create a new one + tracked_robots.push_back(FilteredRobot(robot_detection, ateam_common::TeamColor::Blue)); } + } + for (auto robot_detection : detection_frame_msg->robots_yellow){ + // Check if this is close to an existing measurement (from an id that we have?) + auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [](const FilteredRobot& bot) { + return bot.getId() == robot_detection.robot_id; + }); + if (existing_bot != tracked_robots.end()){ + existing_bot.update(robot_detection); + } else { + // If not, create a new one + tracked_robots.push_back(FilteredRobot(robot_detection, ateam_common::TeamColor::Yellow)); + } + } +}; - void create_new_ball_track(){} +void Camera::create_new_ball_track(){}; - void create_new_robot_track(){} - - private: - int camera_id; - LastFrameInfo last_processed_frame; - std::vector tracked_balls; - std::vector tracked_robots; -}; +void Camera::create_new_robot_track(){}; diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp index 3cafd5ac2..2de876ca2 100644 --- a/new_super_vision_filter/src/camera.hpp +++ b/new_super_vision_filter/src/camera.hpp @@ -36,17 +36,17 @@ class Camera { // Need to process an individual frame // Set geometry from VisionGeometryCameraCalibration.msg // Have a queue/buffer that we can remove old frames/have a set capacity - Camera::Camera(int camera_id); + Camera(int camera_id); - void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg); + void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg); - void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData geometry); + void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData::SharedPtr geometry); void clear_old_messages(); - void process_balls(const ssl_league_msgs::msg::VisionDetectionFrame detection_frame_msg); + void process_balls(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg); - void process_robots(); + void process_robots(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg); void create_new_ball_track(); diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp index 2cae7bd50..d94d8dde4 100644 --- a/new_super_vision_filter/src/filter_types.hpp +++ b/new_super_vision_filter/src/filter_types.hpp @@ -55,10 +55,10 @@ class PosState : public Kalman::Vector static constexpr size_t VX = 2; static constexpr size_t VY = 3; - double const px(){return (*this)[PX]; } - double const py(){return (*this)[PY]; } - double const vx(){return (*this)[VX]; } - double const vy(){return (*this)[VY]; } + const double px(){return (*this)[PX]; } + const double py(){return (*this)[PY]; } + const double vx(){return (*this)[VX]; } + const double vy(){return (*this)[VY]; } }; @@ -69,15 +69,15 @@ class PosState : public Kalman::Vector Curently assumes no measurement noise */ class PosMeasurementModel - : public Kalman::LinearizedMeasurementModel + : public Kalman::LinearizedMeasurementModel { public: // h(x) = predicted measurement PosMeasurement h(const PosState& x) const { PosMeasurement z; - z[0] = x.px(); // px - z[1] = x.py(); // py + z[0] = x(x.PX); // px + z[1] = x(x.PY); // py return z; } @@ -122,7 +122,7 @@ class PosSystemModel : public Kalman::LinearizedSystemModel PosState f(const PosState& x, const Control& /*u*/) const override { PosState x_updated{}; - const auto now = std::chrono::system_clock::now(); + auto now = std::chrono::system_clock::now(); std::chrono::duration dt = now - last_update; // B/c dt is in ms, we need to convert to s, since @@ -142,7 +142,7 @@ class PosSystemModel : public Kalman::LinearizedSystemModel /* Angular position measurement (in rad) */ -class AngleMeasurement : public KalmanVector +class AngleMeasurement : public Kalman::Vector { public: KALMAN_VECTOR(AngleMeasurement, double, 1) @@ -165,8 +165,8 @@ class AngleState : public Kalman::Vector static constexpr size_t PW = 0; static constexpr size_t VW = 1; - double const pw(){return (*this)[PW]; } - double const vw(){return (*this)[VW]; } + const double pw(){return (*this)[PW]; } + const double vw(){return (*this)[VW]; } }; /* @@ -187,7 +187,7 @@ class AngleMeasurementModel } // Jacobian H = ∂h/∂x - void updateJacobians(const StateType& x) override + void updateJacobians(const AngleState& x) override { this->H.setZero(); this->H(0, 0) = 1; // dz_px / d_px diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp index 34b681d25..54eeb7ea1 100644 --- a/new_super_vision_filter/src/filtered_ball.hpp +++ b/new_super_vision_filter/src/filtered_ball.hpp @@ -13,7 +13,7 @@ enum KickState { }; class FilteredBall { - FilteredBall::FilteredBall(ssl_league_msgs::msg::VisionDetectionBall vision_ball); + FilteredBall(ssl_league_msgs::msg::VisionDetectionBall vision_ball); private: KickState currentKickState = ROLLING; @@ -25,7 +25,7 @@ class FilteredBall { Eigen::Vector3 acc; Eigen::Vector2 spin; // Filter - ExtendedKalmanFilter posFilterXY; + Kalman::ExtendedKalmanFilter> posFilterXY; }; #endif // FILTERED_BALL_HPP_ diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index af893c5b1..93e984752 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -110,4 +110,8 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr ateam_msgs::msg::RobotState FilteredRobot::toMsg(){ return ateam_msgs::msg::RobotState{}; -}; \ No newline at end of file +}; + +int FilteredRobot::getId(){ + return bot_id; +} \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 85132629f..b8ebbc1b9 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -30,12 +30,14 @@ #include class FilteredRobot { - FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection_msg, ateam_common::TeamColor team_color); + FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection_msg, ateam_common::TeamColor team_color); void update(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection); ateam_msgs::msg::RobotState toMsg(); + int getId(); + private: int bot_id; ateam_common::TeamColor team; diff --git a/new_super_vision_filter/src/frame_info.hpp b/new_super_vision_filter/src/frame_info.hpp index b9407a8e4..7cecd0073 100644 --- a/new_super_vision_filter/src/frame_info.hpp +++ b/new_super_vision_filter/src/frame_info.hpp @@ -1,6 +1,11 @@ +#ifndef FRAME_INFO_HPP_ +#define FRAME_INFO_HPP_ + #include class LastFrameInfo { public: std::chrono::steady_clock::time_point time_processed; -} \ No newline at end of file +}; + +#endif // FRAME_INFO_HPP_ diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index e7a1c03ad..67cb23fed 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -20,7 +20,7 @@ #include -#include +#include #include "camera.hpp" @@ -37,6 +37,17 @@ class VisionFilterNode : public rclcpp::Node // Output final prediction for robots and ball(s) public: + explicit VisionFilterNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("new_ateam_vision_filter", options), + game_controller_listener_(*this){ + ssl_vision_subscription_ = + create_subscription( + std::string(Topics::kVisionMessages), + 10, + std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1) + ); + + } // Will also need to add publishers here... but want to determine whether we keep existing // approach that is in the old Vision Filter or do something else. @@ -44,28 +55,23 @@ class VisionFilterNode : public rclcpp::Node private: // Subscribe to vision info from our processed messages // in ssl_vision_bridge_node.cpp - rclcpp::Subscription::SharedPtr ssl_vision_subscription_ = - create_subscription( - std::string(Topics::kVisionMessages), - 10, - std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1)); std::map cameras; - void vision_callback(const ssl_league_msgs::msg::vision::VisionWrapper::SharedPtr vision_wrapper_msg) { + void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) { // Add detections to the cameras' msg queues - auto detection = vision_wrapper_msg->detection; + auto detection = vision_wrapper_msg->detection.front(); // Create a new camera if we haven't seen this one before - int detect_camera = detection->camera_id; + int detect_camera = detection.camera_id; if (!(cameras.contains(detect_camera))){ - cameras[detect_camera] = Camera(camera_id); + cameras[detect_camera] = Camera(detect_camera); } cameras[detect_camera].detection_queue.push_back(detection); // Add geometry to the cameras' msg queues - auto geometry = vision_wrapper_msg->geometry; - int geo_camera = geometry->camera_id; + auto geometry = vision_wrapper_msg->geometry.front(); + int geo_camera = geometry.calibration.front().camera_id; if (!(cameras.contains(geo_camera))){ - cameras[geo_camera] = Camera(camera_id); + cameras[geo_camera] = Camera(geo_camera); } return; } @@ -75,6 +81,9 @@ class VisionFilterNode : public rclcpp::Node // Similar to the publish() method in TIGERs return; } + + ateam_common::GameControllerListener game_controller_listener_; + rclcpp::Subscription::SharedPtr ssl_vision_subscription_; }; } // namespace new_super_vision \ No newline at end of file From cc58f2d6949269793ffa820eb396fcaabbf24442 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 3 Jan 2026 22:07:44 -0500 Subject: [PATCH 16/35] Fix camera map creation --- new_super_vision_filter/src/camera.cpp | 16 ++++++----- .../src/vision_filter_node.cpp | 28 ++++++++++++------- 2 files changed, 27 insertions(+), 17 deletions(-) diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp index e66016450..8b8e1ff40 100644 --- a/new_super_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -50,31 +50,33 @@ void Camera::process_balls(const ssl_league_msgs::msg::VisionDetectionFrame::Sha }; void Camera::process_robots(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg){ - for (auto robot_detection : detection_frame_msg->robots_blue){ + for (const auto &robot_detection : detection_frame_msg->robots_blue){ // Check if this is close to an existing measurement (from an id that we have?) - auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [](const FilteredRobot& bot) { + auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), + [&robot_detection](const FilteredRobot& bot) { return bot.getId() == robot_detection.robot_id; }); if (existing_bot != tracked_robots.end()){ - existing_bot.update(robot_detection); + existing_bot->update(robot_detection); } else { // If not, create a new one tracked_robots.push_back(FilteredRobot(robot_detection, ateam_common::TeamColor::Blue)); } } - for (auto robot_detection : detection_frame_msg->robots_yellow){ + for (const auto &robot_detection : detection_frame_msg->robots_yellow){ // Check if this is close to an existing measurement (from an id that we have?) - auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [](const FilteredRobot& bot) { + auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), + [&robot_detection](const FilteredRobot& bot) { return bot.getId() == robot_detection.robot_id; }); if (existing_bot != tracked_robots.end()){ - existing_bot.update(robot_detection); + existing_bot->update(robot_detection); } else { // If not, create a new one tracked_robots.push_back(FilteredRobot(robot_detection, ateam_common::TeamColor::Yellow)); } } -}; +} void Camera::create_new_ball_track(){}; diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 67cb23fed..b65506165 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -59,19 +59,27 @@ class VisionFilterNode : public rclcpp::Node void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) { // Add detections to the cameras' msg queues - auto detection = vision_wrapper_msg->detection.front(); - // Create a new camera if we haven't seen this one before - int detect_camera = detection.camera_id; - if (!(cameras.contains(detect_camera))){ - cameras[detect_camera] = Camera(detect_camera); + if (!vision_wrapper_msg->detection.empty()){ + // Create a new camera if we haven't seen this one before + for (const auto& detection : vision_wrapper_msg->detection){ + int detect_camera = detection.camera_id; + if (!(cameras.contains(detect_camera))){ + cameras.try_emplace(detect_camera, detect_camera); + } + cameras.at(detect_camera).detection_queue.push_back(detection); + } } - cameras[detect_camera].detection_queue.push_back(detection); // Add geometry to the cameras' msg queues - auto geometry = vision_wrapper_msg->geometry.front(); - int geo_camera = geometry.calibration.front().camera_id; - if (!(cameras.contains(geo_camera))){ - cameras[geo_camera] = Camera(geo_camera); + if (!vision_wrapper_msg->geometry.empty()){ + for (const auto& geometry: vision_wrapper_msg->geometry){ + for (const auto& calib : geometry.calibration){ + int geo_camera = calib.camera_id; + if (!(cameras.contains(geo_camera))){ + cameras.try_emplace(geo_camera, geo_camera); + } + } + } } return; } From 4612d1eb0387ea1ca765a134edb778e06bdc7ba6 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 4 Jan 2026 20:33:51 -0500 Subject: [PATCH 17/35] Now building! --- new_super_vision_filter/CMakeLists.txt | 6 +-- new_super_vision_filter/src/camera.cpp | 24 +++++----- new_super_vision_filter/src/camera.hpp | 8 ++-- new_super_vision_filter/src/filter_types.hpp | 45 ++++++++++++++----- new_super_vision_filter/src/filtered_ball.hpp | 23 +++++++++- .../src/filtered_robot.cpp | 23 +++++----- .../src/filtered_robot.hpp | 11 ++--- new_super_vision_filter/test/CMakeLists.txt | 5 +++ new_super_vision_filter/test/filter_test.cpp | 14 +++--- 9 files changed, 106 insertions(+), 53 deletions(-) diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt index 7a61a0a18..5152e66ea 100644 --- a/new_super_vision_filter/CMakeLists.txt +++ b/new_super_vision_filter/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.8) project(new_vision_filter) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +# if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +# add_compile_options(-Wall -Wextra -Wpedantic) +# endif() # find dependencies find_package(ament_cmake REQUIRED) diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp index 8b8e1ff40..c89e8c86d 100644 --- a/new_super_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -29,33 +29,34 @@ Camera::Camera(int camera_id) : camera_id(camera_id) {}; // Set geometry from VisionGeometryCameraCalibration.msg // Have a queue/buffer that we can remove old frames/have a set capacity -void Camera::process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg){ +void Camera::process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg){ process_balls(detection_frame_msg); process_robots(detection_frame_msg); } -void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData::SharedPtr){}; +void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData &geometry){}; void Camera::clear_old_messages(){}; -void Camera::process_balls(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg){ +void Camera::process_balls(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg){ // Geometry msgs stuff... // https://docs.ros2.org/foxy/api/geometry_msgs/index-msg.html - // for (auto ball : detection_frame_msg->balls) { - // // For all of our balls, see if we think this is close to an existing measurement - // // If not, create a new one - // tracked_balls.push_back(FilteredBall(ball)); - // } + for (auto ball : detection_frame_msg.balls) { + // For all of our balls, see if we think this is close to an existing measurement + // If not, create a new one + tracked_balls.push_back(FilteredBall(ball)); + } return; }; -void Camera::process_robots(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg){ - for (const auto &robot_detection : detection_frame_msg->robots_blue){ +void Camera::process_robots(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg){ + for (const auto &robot_detection : detection_frame_msg.robots_blue){ // Check if this is close to an existing measurement (from an id that we have?) auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [&robot_detection](const FilteredRobot& bot) { return bot.getId() == robot_detection.robot_id; }); + if (existing_bot != tracked_robots.end()){ existing_bot->update(robot_detection); } else { @@ -63,12 +64,13 @@ void Camera::process_robots(const ssl_league_msgs::msg::VisionDetectionFrame::Sh tracked_robots.push_back(FilteredRobot(robot_detection, ateam_common::TeamColor::Blue)); } } - for (const auto &robot_detection : detection_frame_msg->robots_yellow){ + for (const auto &robot_detection : detection_frame_msg.robots_yellow){ // Check if this is close to an existing measurement (from an id that we have?) auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), [&robot_detection](const FilteredRobot& bot) { return bot.getId() == robot_detection.robot_id; }); + if (existing_bot != tracked_robots.end()){ existing_bot->update(robot_detection); } else { diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp index 2de876ca2..011d3cef7 100644 --- a/new_super_vision_filter/src/camera.hpp +++ b/new_super_vision_filter/src/camera.hpp @@ -38,15 +38,15 @@ class Camera { // Have a queue/buffer that we can remove old frames/have a set capacity Camera(int camera_id); - void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg); + void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg); - void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData::SharedPtr geometry); + void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData &geometry); void clear_old_messages(); - void process_balls(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg); + void process_balls(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg); - void process_robots(const ssl_league_msgs::msg::VisionDetectionFrame::SharedPtr detection_frame_msg); + void process_robots(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg); void create_new_ball_track(); diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp index d94d8dde4..3ae7fe411 100644 --- a/new_super_vision_filter/src/filter_types.hpp +++ b/new_super_vision_filter/src/filter_types.hpp @@ -1,3 +1,23 @@ +// Copyright 2025 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + #ifndef FILTER_TYPES_HPP_ #define FILTER_TYPES_HPP_ @@ -100,14 +120,14 @@ class PosMeasurementModel class PosSystemModel : public Kalman::LinearizedSystemModel { private: - const float ms_to_s = 1e4; - std::chrono::time_point last_update; + mutable std::chrono::time_point last_update; public: // For now, we don't add any control inputs to the vision system // TODO (Christian): We might need this for the robots, even if we // don't use it for the ball using Control = Kalman::Vector; + using Seconds = std::chrono::duration; /* The f() function applies what would be the A (state transition) @@ -122,15 +142,16 @@ class PosSystemModel : public Kalman::LinearizedSystemModel PosState f(const PosState& x, const Control& /*u*/) const override { PosState x_updated{}; - auto now = std::chrono::system_clock::now(); - std::chrono::duration dt = now - last_update; + auto now = std::chrono::steady_clock::now(); + Seconds dt = now - last_update; + double dt_s = dt.count(); // B/c dt is in ms, we need to convert to s, since // our velocities are all in m/s - x_updated(x.PX) = x(x.PX) + x(x.VX) * dt.count() * ms_to_s; + x_updated(x.PX) = x(x.PX) + x(x.VX) * dt_s; // We assume constant velocity in the system model x_updated(x.VX) = x(x.VX); - x_updated(x.PY) = x(x.PY) + x(x.VY) * dt.count() * ms_to_s; + x_updated(x.PY) = x(x.PY) + x(x.VY) * dt_s; x_updated(x.VY) = x(x.VY); last_update = now; @@ -197,10 +218,11 @@ class AngleMeasurementModel class AngleSystemModel : public Kalman::LinearizedSystemModel { private: - const float ms_to_s = 1e4; - std::chrono::time_point last_update; + mutable std::chrono::time_point last_update; public: + using Control = Kalman::Vector; + using Seconds = std::chrono::duration; /* This is the system's state transition function (applies the A matrix) @@ -214,10 +236,11 @@ class AngleSystemModel : public Kalman::LinearizedSystemModel AngleState f(const AngleState& x, const Control& ) const { AngleState x_updated{}; - const auto now = std::chrono::system_clock::now(); - std::chrono::duration dt = now - last_update; + const auto now = std::chrono::steady_clock::now(); + Seconds dt = now - last_update; + double dt_s = dt.count(); - x_updated(x.PW) = x(x.PW) + x(x.VW) * dt.count() * ms_to_s; + x_updated(x.PW) = x(x.PW) + x(x.VW) * dt_s; return x_updated; } diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp index 54eeb7ea1..abbc3529b 100644 --- a/new_super_vision_filter/src/filtered_ball.hpp +++ b/new_super_vision_filter/src/filtered_ball.hpp @@ -1,3 +1,23 @@ +// Copyright 2025 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + #ifndef FILTERED_BALL_HPP_ #define FILTERED_BALL_HPP_ @@ -13,7 +33,8 @@ enum KickState { }; class FilteredBall { - FilteredBall(ssl_league_msgs::msg::VisionDetectionBall vision_ball); + public: + FilteredBall(ssl_league_msgs::msg::VisionDetectionBall &vision_ball); private: KickState currentKickState = ROLLING; diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 93e984752..80d9550dd 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 A Team +// Copyright 2025 A Team // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -17,6 +17,7 @@ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. + #include "filtered_robot.hpp" #include "filter_types.hpp" @@ -28,14 +29,14 @@ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ // OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp -FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection_msg, ateam_common::TeamColor team_color) - : posFilterXY(), posFilterW(), bot_id(robot_detection_msg->robot_id), height(robot_detection_msg->height * 1000), team(team_color) { +FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color) + : posFilterXY(), posFilterW(), bot_id(robot_detection_msg.robot_id), height(robot_detection_msg.height * 1000), team(team_color) { // TODO (Christian) - Might need to change the below to use our state/measurement types // Initialize XY KF PosState initial_state_xy; initial_state_xy << - robot_detection_msg->pose.position.x, - robot_detection_msg->pose.position.y, + robot_detection_msg.pose.position.x, + robot_detection_msg.pose.position.y, 0, 0; posFilterXY.init(initial_state_xy); @@ -54,7 +55,7 @@ FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot::SharedP */ AngleState initial_state_w; initial_state_w << - robot_detection_msg->pose.orientation.w, + robot_detection_msg.pose.orientation.w, 0; posFilterW.init(initial_state_w); /* @@ -67,7 +68,7 @@ FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot::SharedP posFilterW.setCovariance(w_covariance); } -void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection_msg) { +void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) ++age; @@ -89,13 +90,13 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr auto w_pred = posFilterW.predict(systemModelW); PosState pos; pos << - robot_detection_msg->pose.position.x, - robot_detection_msg->pose.position.y, + robot_detection_msg.pose.position.x, + robot_detection_msg.pose.position.y, 0, 0; AngleState angle; angle << - robot_detection_msg->pose.orientation.w, + robot_detection_msg.pose.orientation.w, 0; PosMeasurement xy_measurement = measurementModelXY.h(pos); AngleMeasurement w_measurement = measurementModelW.h(angle); @@ -112,6 +113,6 @@ ateam_msgs::msg::RobotState FilteredRobot::toMsg(){ return ateam_msgs::msg::RobotState{}; }; -int FilteredRobot::getId(){ +int FilteredRobot::getId() const { return bot_id; } \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index b8ebbc1b9..2dfb8be4f 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 A Team +// Copyright 2025 A Team // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -30,13 +30,14 @@ #include class FilteredRobot { - FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection_msg, ateam_common::TeamColor team_color); + public: + FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color); - void update(ssl_league_msgs::msg::VisionDetectionRobot::SharedPtr robot_detection); + void update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection); - ateam_msgs::msg::RobotState toMsg(); + ateam_msgs::msg::RobotState toMsg(); - int getId(); + int getId() const; private: int bot_id; diff --git a/new_super_vision_filter/test/CMakeLists.txt b/new_super_vision_filter/test/CMakeLists.txt index 0edaf0359..4ee4f9b39 100644 --- a/new_super_vision_filter/test/CMakeLists.txt +++ b/new_super_vision_filter/test/CMakeLists.txt @@ -1,3 +1,8 @@ find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(filter_test filter_test.cpp) + +target_include_directories(filter_test PUBLIC + $ +) + target_link_libraries(filter_test ${PROJECT_NAME}) diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp index 1d9004acd..7ae86e72b 100644 --- a/new_super_vision_filter/test/filter_test.cpp +++ b/new_super_vision_filter/test/filter_test.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 A Team +// Copyright 2026 A Team // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -37,21 +37,21 @@ class FilteredRobotTest : public ::testing::Test team }; } -} +}; -TEST_F(RobotSetupTest, InitalState) +TEST(RobotSetupTest, InitalState) { // Check that initial state is valid return; } -TEST_F(RobotSetupTest, InitialCovariance) +TEST(RobotSetupTest, InitialCovariance) { // Check that initial covariance is valid return; } -TEST_F(RobotUpdateTest, WaitUntilOldEnough) +TEST(RobotUpdateTest, WaitUntilOldEnough) { // Create valid timestamp and fake measurement // Try to update @@ -61,7 +61,7 @@ TEST_F(RobotUpdateTest, WaitUntilOldEnough) return; } -TEST_F(RobotUpdateTest, UpdateIfTimestampValid) +TEST(RobotUpdateTest, UpdateIfTimestampValid) { // Get initial estimate // Create valid timestamp and fake measurement @@ -70,7 +70,7 @@ TEST_F(RobotUpdateTest, UpdateIfTimestampValid) return; } -TEST_F(RobotUpdateTest, DontUpdateIfTimestampInvalid) +TEST(RobotUpdateTest, DontUpdateIfTimestampInvalid) { // Get initial estimate // Create invalid timestamp and fake measurement From 316c3516511885308f3982bee9b372027c91c6ba Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 4 Jan 2026 20:50:18 -0500 Subject: [PATCH 18/35] Remove unused extra queue --- new_super_vision_filter/src/camera.hpp | 4 ---- new_super_vision_filter/src/vision_filter_node.cpp | 2 +- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp index 011d3cef7..50ff768d6 100644 --- a/new_super_vision_filter/src/camera.hpp +++ b/new_super_vision_filter/src/camera.hpp @@ -21,7 +21,6 @@ #ifndef CAMERA_HPP_ #define CAMERA_HPP_ -#include #include #include #include @@ -52,9 +51,6 @@ class Camera { void create_new_robot_track(); - std::deque detection_queue; - std::deque geometry_queue; - private: int camera_id; LastFrameInfo last_processed_frame; diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index b65506165..04a42858e 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -66,7 +66,7 @@ class VisionFilterNode : public rclcpp::Node if (!(cameras.contains(detect_camera))){ cameras.try_emplace(detect_camera, detect_camera); } - cameras.at(detect_camera).detection_queue.push_back(detection); + cameras.at(detect_camera).process_detection_frame(detection); } } From f1125a894b57f1344f505ca2cf4581cf9c2105c4 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 6 Jan 2026 22:25:13 -0500 Subject: [PATCH 19/35] Change to using tracks on ros node rather than per camera --- new_super_vision_filter/src/camera.cpp | 4 +- new_super_vision_filter/src/camera.hpp | 6 +- .../src/filtered_robot.cpp | 1 + .../src/filtered_robot.hpp | 4 +- .../src/measurements/ball_track.hpp | 42 ++++++++++ .../src/measurements/robot_track.hpp | 50 ++++++++++++ .../src/tracker/ball_tracker.hpp | 0 .../src/tracker/robot_tracker.hpp | 0 .../src/vision_filter_node.cpp | 76 ++++++++++++++----- 9 files changed, 159 insertions(+), 24 deletions(-) create mode 100644 new_super_vision_filter/src/measurements/ball_track.hpp create mode 100644 new_super_vision_filter/src/measurements/robot_track.hpp delete mode 100644 new_super_vision_filter/src/tracker/ball_tracker.hpp delete mode 100644 new_super_vision_filter/src/tracker/robot_tracker.hpp diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp index c89e8c86d..701e1f171 100644 --- a/new_super_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -67,8 +67,8 @@ void Camera::process_robots(const ssl_league_msgs::msg::VisionDetectionFrame &de for (const auto &robot_detection : detection_frame_msg.robots_yellow){ // Check if this is close to an existing measurement (from an id that we have?) auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), - [&robot_detection](const FilteredRobot& bot) { - return bot.getId() == robot_detection.robot_id; + [&robot_detection](const RobotTrack& track) { + return track.bot_id == robot_detection.robot_id; }); if (existing_bot != tracked_robots.end()){ diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp index 50ff768d6..ada75be45 100644 --- a/new_super_vision_filter/src/camera.hpp +++ b/new_super_vision_filter/src/camera.hpp @@ -53,9 +53,9 @@ class Camera { private: int camera_id; - LastFrameInfo last_processed_frame; - std::vector tracked_balls; - std::vector tracked_robots; + std::chrono::time_point last_updated; + // std::vector tracked_balls; + // std::vector tracked_robots; }; #endif // CAMERA_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 80d9550dd..21d057c78 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -74,6 +74,7 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_dete ++age; bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter + // TODO (Christian) - do this for each current track in the queue const std::chrono::time_point now = std::chrono::system_clock::now(); // If it's been too long, don't use this message diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 2dfb8be4f..c16f3aae8 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -47,8 +47,8 @@ class FilteredRobot { int maxHealth = 20; double maxDistance = -1.0; std::chrono::milliseconds update_threshold{50}; - std::chrono::time_point timestamp; - std::chrono::time_point last_visible_timestamp; + std::chrono::time_point timestamp; + std::chrono::time_point last_visible_timestamp; double height; // in m // Use the below filtered values when getting X/Y/w (theta) and // velocities diff --git a/new_super_vision_filter/src/measurements/ball_track.hpp b/new_super_vision_filter/src/measurements/ball_track.hpp new file mode 100644 index 000000000..b9aad1436 --- /dev/null +++ b/new_super_vision_filter/src/measurements/ball_track.hpp @@ -0,0 +1,42 @@ +// Copyright 2025 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BALL_TRACK_HPP_ +#define BALL_TRACK_HPP_ + +#include "filter_types.hpp" + +#include + +class BallTrack { + public: + BallTrack(ssl_league_msgs::msg:VisionDetectionBall &ball_detection, int &camera_id) : camera_id(camera_id) { + PosMeasurement pos; + pos << ball_detection.pos.x, + ball_detection.pos.y; + timestamp = std::chrono::steady_clock::now(); + } + + PosMeasurement pos; + std::chrono::time_point timestamp; + int camera_id; +} + +#endif // BALL_TRACK_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/measurements/robot_track.hpp b/new_super_vision_filter/src/measurements/robot_track.hpp new file mode 100644 index 000000000..c628b9dfc --- /dev/null +++ b/new_super_vision_filter/src/measurements/robot_track.hpp @@ -0,0 +1,50 @@ +// Copyright 2025 A Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef ROBOT_TRACK_HPP_ +#define ROBOT_TRACK_HPP_ + +#include +#include + +#include "filter_types.hpp" + +class RobotTrack { + RobotTrack( + ssl_league_msgs::msg::VisionDetectionRobot &bot_detection, + int &camera_id, + ateam_common::TeamColor &team + ) : camera_id(camera_id), team(team) { + PosMeasurement pos; + pos << bot_detection.pose.position.x, + bot_detection.pose.position.y; + angle << bot_detection.pose.orientation.w; + robot_id = bot_detection.robot_id; + } + + PosMeasurement pos; + AngleMeasurement angle; + std::chrono::time_point timestamp; + int camera_id; + ateam_common::TeamColor team; + int robot_id; +} + +#endif // ROBOT_TRACK_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/tracker/ball_tracker.hpp b/new_super_vision_filter/src/tracker/ball_tracker.hpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/new_super_vision_filter/src/tracker/robot_tracker.hpp b/new_super_vision_filter/src/tracker/robot_tracker.hpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 04a42858e..64fd3cad4 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -17,12 +17,16 @@ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#include "camera.hpp" +#include "filtered_robot.hpp" +#include "filtered_ball.hpp" +#include "measurements/ball_track.hpp" +#include "measurements/robot_track.hpp" #include - #include - -#include "camera.hpp" +#include +#include namespace new_super_vision { @@ -57,30 +61,68 @@ class VisionFilterNode : public rclcpp::Node // in ssl_vision_bridge_node.cpp std::map cameras; + std::vector blue_robots; + std::vector yellow_robots; + + std::vector ball_tracks; + std::vector blue_tracks; + std::vector yellow_tracks; + void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) { - // Add detections to the cameras' msg queues + // Add detections to the queues if (!vision_wrapper_msg->detection.empty()){ - // Create a new camera if we haven't seen this one before for (const auto& detection : vision_wrapper_msg->detection){ int detect_camera = detection.camera_id; + // Create a new camera if we haven't seen this one before if (!(cameras.contains(detect_camera))){ cameras.try_emplace(detect_camera, detect_camera); } - cameras.at(detect_camera).process_detection_frame(detection); - } - } - - // Add geometry to the cameras' msg queues - if (!vision_wrapper_msg->geometry.empty()){ - for (const auto& geometry: vision_wrapper_msg->geometry){ - for (const auto& calib : geometry.calibration){ - int geo_camera = calib.camera_id; - if (!(cameras.contains(geo_camera))){ - cameras.try_emplace(geo_camera, geo_camera); - } + + // Create a track from each robot in this message + for (const auto& bot : detection.yellow_robots) { + yellow_tracks.push_back( + RobotTrack(bot, detect_camera, ateam_common::TeamColor::Yellow) + ); } + + for (const auto& bot : detection.blue_robots) { + blue_tracks.push_back( + RobotTrack(bot, detect_camera, ateam_common::TeamColor::Blue) + ); + } + + // Create a track from each ball in this message + for (const auto& ball: detection.balls) { + ball_tracks.push_back( + BallTrack(ball, detect_camera) + ); + } + } + + // Process all the new updates from the tracks + // TODO (Christian) - Filter out known bad tracks first + for (const auto& bot_track : blue_tracks){ + + } + for (const auto& bot_track : yellow_tracks){ + + } + for (const auto& ball_track : ball_tracks){ + } } + + // // Add geometry to the cameras' msg queues + // if (!vision_wrapper_msg->geometry.empty()){ + // for (const auto& geometry: vision_wrapper_msg->geometry){ + // for (const auto& calib : geometry.calibration){ + // int geo_camera = calib.camera_id; + // if (!(cameras.contains(geo_camera))){ + // cameras.try_emplace(geo_camera, geo_camera); + // } + // } + // } + // } return; } From 44cf585fde12937a4b310a10a70dfac45a5c137c Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 11 Jan 2026 22:48:04 -0500 Subject: [PATCH 20/35] Minor updates and notes on next TODOs --- .../src/filtered_robot.cpp | 34 ++++------- .../src/filtered_robot.hpp | 4 +- .../src/kick/basic_chip.hpp | 1 - .../src/kick/basic_kick.hpp | 1 - .../src/kick/kick_detector.hpp | 1 - .../src/measurements/robot_track.hpp | 4 ++ .../src/vision_filter_node.cpp | 60 +++++++++++++------ 7 files changed, 57 insertions(+), 48 deletions(-) delete mode 100644 new_super_vision_filter/src/kick/basic_chip.hpp delete mode 100644 new_super_vision_filter/src/kick/basic_kick.hpp delete mode 100644 new_super_vision_filter/src/kick/kick_detector.hpp diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 21d057c78..802f07f84 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -20,6 +20,7 @@ #include "filtered_robot.hpp" #include "filter_types.hpp" +#include "measurements/robot_track.hpp" #include #include @@ -29,14 +30,14 @@ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ // OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp -FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color) - : posFilterXY(), posFilterW(), bot_id(robot_detection_msg.robot_id), height(robot_detection_msg.height * 1000), team(team_color) { +FilteredRobot::FilteredRobot(RobotTrack track, ateam_common::TeamColor team_color) + : posFilterXY(), posFilterW(), bot_id(track.robot_id), team(team_color) { // TODO (Christian) - Might need to change the below to use our state/measurement types // Initialize XY KF PosState initial_state_xy; initial_state_xy << - robot_detection_msg.pose.position.x, - robot_detection_msg.pose.position.y, + track.pos.px(), + track.pos.py(), 0, 0; posFilterXY.init(initial_state_xy); @@ -55,7 +56,7 @@ FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_de */ AngleState initial_state_w; initial_state_w << - robot_detection_msg.pose.orientation.w, + track.angle.pw(), 0; posFilterW.init(initial_state_w); /* @@ -68,7 +69,7 @@ FilteredRobot::FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_de posFilterW.setCovariance(w_covariance); } -void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg) { +void FilteredRobot::update(RobotTrack &track) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) ++age; @@ -78,36 +79,21 @@ void FilteredRobot::update(ssl_league_msgs::msg::VisionDetectionRobot robot_dete const std::chrono::time_point now = std::chrono::system_clock::now(); // If it's been too long, don't use this message - if (now - timestamp > update_threshold || is_new) { - timestamp = now; + if (now - track.timestamp > update_threshold || is_new) { return; } - // Update our timestamp for next time - timestamp = now; // Predict state forward // Predict covariance forward // (All encompassed by the .predict() function) auto xy_pred = posFilterXY.predict(systemModelXY); auto w_pred = posFilterW.predict(systemModelW); - PosState pos; - pos << - robot_detection_msg.pose.position.x, - robot_detection_msg.pose.position.y, - 0, - 0; - AngleState angle; - angle << - robot_detection_msg.pose.orientation.w, - 0; - PosMeasurement xy_measurement = measurementModelXY.h(pos); - AngleMeasurement w_measurement = measurementModelW.h(angle); // Update the Jacobian matrix (contained in filter) // Compute Kalman gain (contained in filter) // Update state estimate (returned) // Update covariance estimate (contained in filter) // All encompassed by the .update() function - posXYEstimate = posFilterXY.update(measurementModelXY, xy_measurement); - posWEstimate = posFilterW.update(measurementModelW, w_measurement); + posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); + posWEstimate = posFilterW.update(measurementModelW, track.angle); } ateam_msgs::msg::RobotState FilteredRobot::toMsg(){ diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index c16f3aae8..b44ceb7f6 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -49,18 +49,16 @@ class FilteredRobot { std::chrono::milliseconds update_threshold{50}; std::chrono::time_point timestamp; std::chrono::time_point last_visible_timestamp; - double height; // in m + // double height; // in m // Use the below filtered values when getting X/Y/w (theta) and // velocities // X, Y Kalman::ExtendedKalmanFilter posFilterXY; PosSystemModel systemModelXY; - PosMeasurementModel measurementModelXY; PosState posXYEstimate{}; // Theta Kalman::ExtendedKalmanFilter posFilterW; AngleSystemModel systemModelW; - AngleMeasurementModel measurementModelW; AngleState posWEstimate{}; }; diff --git a/new_super_vision_filter/src/kick/basic_chip.hpp b/new_super_vision_filter/src/kick/basic_chip.hpp deleted file mode 100644 index f5a2315f3..000000000 --- a/new_super_vision_filter/src/kick/basic_chip.hpp +++ /dev/null @@ -1 +0,0 @@ -// Some sort of basic (non?)linear model to predict the expected trajectory of a chip that leaves the ground \ No newline at end of file diff --git a/new_super_vision_filter/src/kick/basic_kick.hpp b/new_super_vision_filter/src/kick/basic_kick.hpp deleted file mode 100644 index 28c26b811..000000000 --- a/new_super_vision_filter/src/kick/basic_kick.hpp +++ /dev/null @@ -1 +0,0 @@ -// Some sort of basic linear model to predict the expected trajectory of a kick on the ground \ No newline at end of file diff --git a/new_super_vision_filter/src/kick/kick_detector.hpp b/new_super_vision_filter/src/kick/kick_detector.hpp deleted file mode 100644 index 619d1a2b2..000000000 --- a/new_super_vision_filter/src/kick/kick_detector.hpp +++ /dev/null @@ -1 +0,0 @@ -// Detects kicks and chips diff --git a/new_super_vision_filter/src/measurements/robot_track.hpp b/new_super_vision_filter/src/measurements/robot_track.hpp index c628b9dfc..a2cba1b02 100644 --- a/new_super_vision_filter/src/measurements/robot_track.hpp +++ b/new_super_vision_filter/src/measurements/robot_track.hpp @@ -26,6 +26,10 @@ #include "filter_types.hpp" +/** + * @brief Robot position and angle measurement from a single camera detection, + * used to provide updates to the Kalman filter. + */ class RobotTrack { RobotTrack( ssl_league_msgs::msg::VisionDetectionRobot &bot_detection, diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 64fd3cad4..f46b46941 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include namespace new_super_vision { @@ -53,8 +55,7 @@ class VisionFilterNode : public rclcpp::Node } - // Will also need to add publishers here... but want to determine whether we keep existing - // approach that is in the old Vision Filter or do something else. + // Will also need to add publishers here and wall clock timer private: // Subscribe to vision info from our processed messages @@ -63,6 +64,7 @@ class VisionFilterNode : public rclcpp::Node std::vector blue_robots; std::vector yellow_robots; + std::optional ball; std::vector ball_tracks; std::vector blue_tracks; @@ -99,36 +101,58 @@ class VisionFilterNode : public rclcpp::Node } } + // Sort our tracks to make sure they are in order of the time received + // in case created/processed them out of order + // Process all the new updates from the tracks - // TODO (Christian) - Filter out known bad tracks first for (const auto& bot_track : blue_tracks){ - + auto it = std::find_if( + blue_robots.begin(), + blue_robots.end(), + [](){ return bot_track.robot_id == bot.getId(); } + ); + if (it != blue_robots.end()){ + blue_robots.push_back( + FilteredRobot(bot_track, ateam_common::TeamColor::Blue) + ); + } else { + it->update(bot_track); + } } for (const auto& bot_track : yellow_tracks){ - + auto it = std::find_if( + yellow_robots.begin(), + yellow_robots.end(), + [](){ return bot_track.robot_id == bot.getId(); } + ); + if (it != yellow_robots.end()){ + yellow_robots.push_back( + FilteredRobot(bot_track, ateam_common::TeamColor::Yellow) + ); + } else { + it->update(bot_track); + } } for (const auto& ball_track : ball_tracks){ - + if (!ball.has_value()){ + ball = FilteredBall(ball_track); + } else { + ball->update(ball_track); + } } } - // // Add geometry to the cameras' msg queues - // if (!vision_wrapper_msg->geometry.empty()){ - // for (const auto& geometry: vision_wrapper_msg->geometry){ - // for (const auto& calib : geometry.calibration){ - // int geo_camera = calib.camera_id; - // if (!(cameras.contains(geo_camera))){ - // cameras.try_emplace(geo_camera, geo_camera); - // } - // } - // } - // } return; } void timer_callback() { + // Get the current predictions for all robots and ball(s) + // Make sure these specific critera... + // Should be recently updated enough - prune stale robots/balls + // Pick the best ball if multiple exist (based on whichever is closest to our most recent + // prediction?) + // // Publish the updated vision info - // Similar to the publish() method in TIGERs return; } From 11974658db438967245c0821a7ed9c0317eeff89 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 18 Jan 2026 22:56:50 -0500 Subject: [PATCH 21/35] Add init function and update to filtered ball --- new_super_vision_filter/src/filtered_ball.cpp | 49 +++++++++++++++++++ new_super_vision_filter/src/filtered_ball.hpp | 18 ++++--- .../src/filtered_robot.cpp | 10 ++-- .../src/filtered_robot.hpp | 4 +- .../src/vision_filter_node.cpp | 2 + 5 files changed, 71 insertions(+), 12 deletions(-) create mode 100644 new_super_vision_filter/src/filtered_ball.cpp diff --git a/new_super_vision_filter/src/filtered_ball.cpp b/new_super_vision_filter/src/filtered_ball.cpp new file mode 100644 index 000000000..96894db02 --- /dev/null +++ b/new_super_vision_filter/src/filtered_ball.cpp @@ -0,0 +1,49 @@ +#include "filtered_ball.hpp" + +FilteredBall::FilteredBall(ssl_league_msgs::msg::VisionDetectionBall &ball_detection_msg){ + PosState initial_state_xy; + initial_state_xy << + track.pos.px(), + track.pos.py(), + 0, + 0; + posFilterXY.init(initial_state_xy); + Kalman::Matrix xy_covariance; + // This is in m, so initial covariance is 100 mm. + // We don't get a velocity input in the measurement itself, + // so that has a large initial uncertainty. + xy_covariance << 1e-2, 0, 0, 0, + 0, 1e-2, 0, 0, + 0, 0, 1e3, 0, + 0, 0, 0, 1e3; + posFilterXY.setCovariance(xy_covariance); +} + +void FilteredBall::update(BallTrack &track){ + // Make sure this detection isn't crazy off from our previous ones + // (unless our filter is still new/only has a few measurements) + ++age; + bool is_new = age < oldEnough; + // As long as its reasonable, update the Kalman Filter + const std::chrono::time_point now = + std::chrono::system_clock::now(); + // If it's been too long, don't use this message + if (now - track.timestamp > update_threshold || is_new) { + return; + } + // Predict state forward + // Predict covariance forward + // (All encompassed by the .predict() function) + auto xy_pred = posFilterXY.predict(systemModelXY); + // Update the Jacobian matrix (contained in filter) + // Compute Kalman gain (contained in filter) + // Update state estimate (returned) + // Update covariance estimate (contained in filter) + // All encompassed by the .update() function + posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); +} + +ateam_msgs::msg::BallState FilteredBall::toMsg(){ + ateam_msgs::msg::BallState ball_state_msg{}; + return ball_state_msg; +} \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp index abbc3529b..cebb36abe 100644 --- a/new_super_vision_filter/src/filtered_ball.hpp +++ b/new_super_vision_filter/src/filtered_ball.hpp @@ -24,7 +24,9 @@ #include #include #include +#include #include "kalman/ExtendedKalmanFilter.hpp" +#include "filter_types.hpp" enum KickState { ROLLING, @@ -34,17 +36,21 @@ enum KickState { class FilteredBall { public: - FilteredBall(ssl_league_msgs::msg::VisionDetectionBall &vision_ball); + FilteredBall(ssl_league_msgs::msg::VisionDetectionBall &ball_detection_msg); + + void update(ssl_league_msgs::msg::VisionDetectionBall &ball_detection) + + ateam_msgs::msg::BallState toMsg(); private: + int age = 0; + int oldEnough = 3; + int maxHealth = 20; + double maxDistance = -1.0; KickState currentKickState = ROLLING; + std::chrono::milliseconds update_threshold{50}; std::chrono::steady_clock::time_point timestamp; std::chrono::steady_clock::time_point last_visible_timestamp; - // TODO: Add unit labels/info - Eigen::Vector3 pos; - Eigen::Vector3 vel; - Eigen::Vector3 acc; - Eigen::Vector2 spin; // Filter Kalman::ExtendedKalmanFilter> posFilterXY; }; diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 802f07f84..d7de0b4aa 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -30,9 +30,8 @@ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ // OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp -FilteredRobot::FilteredRobot(RobotTrack track, ateam_common::TeamColor team_color) +FilteredRobot::FilteredRobot(RobotTrack &track, ateam_common::TeamColor team_color) : posFilterXY(), posFilterW(), bot_id(track.robot_id), team(team_color) { - // TODO (Christian) - Might need to change the below to use our state/measurement types // Initialize XY KF PosState initial_state_xy; initial_state_xy << @@ -42,6 +41,9 @@ FilteredRobot::FilteredRobot(RobotTrack track, ateam_common::TeamColor team_colo 0; posFilterXY.init(initial_state_xy); Kalman::Matrix xy_covariance; + // This is in m, so initial covariance is 100 mm. + // We don't get a velocity input in the measurement itself, + // so that has a large initial uncertainty. xy_covariance << 1e-2, 0, 0, 0, 0, 1e-2, 0, 0, 0, 0, 1e3, 0, @@ -75,7 +77,6 @@ void FilteredRobot::update(RobotTrack &track) { ++age; bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter - // TODO (Christian) - do this for each current track in the queue const std::chrono::time_point now = std::chrono::system_clock::now(); // If it's been too long, don't use this message @@ -97,7 +98,8 @@ void FilteredRobot::update(RobotTrack &track) { } ateam_msgs::msg::RobotState FilteredRobot::toMsg(){ - return ateam_msgs::msg::RobotState{}; + ateam_msgs::msg::RobotState robot_state_msg{}; + return robot_state_msg; }; int FilteredRobot::getId() const { diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index b44ceb7f6..98501585b 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -27,11 +27,11 @@ #include #include #include -#include +#include class FilteredRobot { public: - FilteredRobot(ssl_league_msgs::msg::VisionDetectionRobot robot_detection_msg, ateam_common::TeamColor team_color); + FilteredRobot(RobotTrack &track, ateam_common::TeamColor team_color); void update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection); diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index f46b46941..0ea3d180c 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -53,6 +53,7 @@ class VisionFilterNode : public rclcpp::Node std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1) ); + timer_ = create_wall_timer(10ms, std::bind(&VisionFilterNode::timer_callback, this)); } // Will also need to add publishers here and wall clock timer @@ -61,6 +62,7 @@ class VisionFilterNode : public rclcpp::Node // Subscribe to vision info from our processed messages // in ssl_vision_bridge_node.cpp std::map cameras; + rclcpp::TimerBase::SharedPtr timer_; std::vector blue_robots; std::vector yellow_robots; From 4aa1afed99920f8a32fbc5a56dd170566f693e8f Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 18 Jan 2026 23:08:24 -0500 Subject: [PATCH 22/35] Add publishers, subscribers, other ROS stuff to new filter node --- .../src/vision_filter_node.cpp | 65 +++++++++++++++++-- 1 file changed, 59 insertions(+), 6 deletions(-) diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 0ea3d180c..2fa11c28a 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -54,13 +54,43 @@ class VisionFilterNode : public rclcpp::Node ); timer_ = create_wall_timer(10ms, std::bind(&VisionFilterNode::timer_callback, this)); + + ball_publisher_ = create_publisher( + std::string(Topics::kBall), + rclcpp::SystemDefaultsQoS()); + + + ateam_common::indexed_topic_helpers::create_indexed_publishers + ( + blue_robots_publisher_, + Topics::kBlueTeamRobotPrefix, + rclcpp::SystemDefaultsQoS(), + this + ); + ateam_common::indexed_topic_helpers::create_indexed_publishers + ( + yellow_robots_publisher_, + Topics::kYellowTeamRobotPrefix, + rclcpp::SystemDefaultsQoS(), + this + ); + + ssl_vision_subscription_ = + create_subscription( + std::string(Topics::kVisionMessages), + 10, + std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1)); + + field_subscription_ = + create_subscription( + std::string(Topics::kField), + 10, + std::bind(&VisionFilterNode::field_callback, this, std::placeholders::_1)); } // Will also need to add publishers here and wall clock timer private: - // Subscribe to vision info from our processed messages - // in ssl_vision_bridge_node.cpp std::map cameras; rclcpp::TimerBase::SharedPtr timer_; @@ -71,6 +101,19 @@ class VisionFilterNode : public rclcpp::Node std::vector ball_tracks; std::vector blue_tracks; std::vector yellow_tracks; + + // All this stuff interacts with other nodes (pub/sub related) + std::array::SharedPtr, + 16> blue_robots_publisher_; + std::array::SharedPtr, + 16> yellow_robots_publisher_; + + ateam_common::GameControllerListener game_controller_listener_; + rclcpp::Subscription::SharedPtr ssl_vision_subscription_; + rclcpp::Publisher::SharedPtr vision_state_publisher_; + // The two below are in case we are sharing a field during testing at event + rclcpp::Subscription::SharedPtr field_subscription_; + int ignore_side_; void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) { // Add detections to the queues @@ -157,9 +200,19 @@ class VisionFilterNode : public rclcpp::Node // Publish the updated vision info return; } - - ateam_common::GameControllerListener game_controller_listener_; - rclcpp::Subscription::SharedPtr ssl_vision_subscription_; + + void field_callback( + const ateam_msgs::msg::FieldInfo::SharedPtr field_info_msg) + { + const auto team_side = game_controller_listener_.GetTeamSide(); + if (team_side == ateam_common::TeamSide::PositiveHalf) { + ignore_side_ = -field_info_msg->ignore_side; + } else { + ignore_side_ = field_info_msg->ignore_side; + } + } }; -} // namespace new_super_vision \ No newline at end of file +} // namespace new_super_vision + +RCLCPP_COMPONENTS_REGISTER_NODE(new_super_vision::VisionFilterNode) From dd2331d58b05d5f6381ba05bced86d24fc16a23f Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 1 Feb 2026 22:07:08 -0500 Subject: [PATCH 23/35] Ready to work on compilation then test --- new_super_vision_filter/src/filtered_ball.cpp | 26 ++++++++-- new_super_vision_filter/src/filtered_ball.hpp | 5 +- .../src/filtered_robot.cpp | 39 ++++++++++++++- .../src/filtered_robot.hpp | 2 + .../src/vision_filter_node.cpp | 50 ++++++++++++++++--- 5 files changed, 109 insertions(+), 13 deletions(-) diff --git a/new_super_vision_filter/src/filtered_ball.cpp b/new_super_vision_filter/src/filtered_ball.cpp index 96894db02..6c1d8f1e2 100644 --- a/new_super_vision_filter/src/filtered_ball.cpp +++ b/new_super_vision_filter/src/filtered_ball.cpp @@ -22,7 +22,12 @@ FilteredBall::FilteredBall(ssl_league_msgs::msg::VisionDetectionBall &ball_detec void FilteredBall::update(BallTrack &track){ // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) - ++age; + if (age < oldEnough) { + ++age; + } + if (health < maxHealth){ + health += 2; + } bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter const std::chrono::time_point now = @@ -43,7 +48,22 @@ void FilteredBall::update(BallTrack &track){ posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); } -ateam_msgs::msg::BallState FilteredBall::toMsg(){ - ateam_msgs::msg::BallState ball_state_msg{}; +ateam_msgs::msg::VisionStateBall FilteredBall::toMsg(){ + ateam_msgs::msg::VisionStateBall ball_state_msg{}; + bool is_new = age < oldEnough; + + if (health > 0 && !is_new) { + // NOTE: Does not contain acceleration info + ball_state_msg.visible = true; + ball_detection_msg.pose.position.x = posXYEstimate.px(); + ball_detection_msg.pose.position.y = posXYEstimate.py(); + ball_detection_msg.pose.twist.linear.x = posXYEstimate.vx(); + ball_detection_msg.pose.twist.linear.y = posXYEstimate.vy(); + --health; + } return ball_state_msg; +} + +bool FilteredBall::isHealthy(){ + return health > 0; } \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp index cebb36abe..782bac4b0 100644 --- a/new_super_vision_filter/src/filtered_ball.hpp +++ b/new_super_vision_filter/src/filtered_ball.hpp @@ -42,17 +42,20 @@ class FilteredBall { ateam_msgs::msg::BallState toMsg(); + bool isHealthy() const; + private: int age = 0; int oldEnough = 3; int maxHealth = 20; + int health = 2; double maxDistance = -1.0; KickState currentKickState = ROLLING; std::chrono::milliseconds update_threshold{50}; std::chrono::steady_clock::time_point timestamp; std::chrono::steady_clock::time_point last_visible_timestamp; // Filter - Kalman::ExtendedKalmanFilter> posFilterXY; + Kalman::ExtendedKalmanFilter posFilterXY; }; #endif // FILTERED_BALL_HPP_ diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index d7de0b4aa..ba3a85318 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -25,6 +25,7 @@ #include #include #include +#include // See https://thekalmanfilter.com/extended-kalman-filter-python-example/ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ @@ -74,7 +75,12 @@ FilteredRobot::FilteredRobot(RobotTrack &track, ateam_common::TeamColor team_col void FilteredRobot::update(RobotTrack &track) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) - ++age; + if (age < oldEnough) { + ++age; + } + if (health < maxHealth){ + health += 2; + } bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter const std::chrono::time_point now = @@ -98,10 +104,39 @@ void FilteredRobot::update(RobotTrack &track) { } ateam_msgs::msg::RobotState FilteredRobot::toMsg(){ - ateam_msgs::msg::RobotState robot_state_msg{}; + ateam_msgs::msg::VisionStateRobot robot_state_msg{}; + bool is_new = age < oldEnough; + + if (health > 0 && !is_new) { + robot_state_msg.visible = true; + robot_detection_msg.pose.position.x = posXYEstimate.px(); + robot_detection_msg.pose.position.y = posXYEstimate.py(); + robot_detection_msg.pose.twist.linear.x = posXYEstimate.vx(); + robot_detection_msg.pose.twist.linear.y = posXYEstimate.vy(); + + robot_detection_msg.pose.orientation.x = 0; + robot_detection_msg.pose.orientation.y = 0; + robot_detection_msg.pose.orientation.z = 1; + robot_detection_msg.pose.orientation.w = posWEstimate.pw(); + robot_detection_msg.twist.angular.z = posWEstimate.vw(); + + // Convert to body velocities for plotting/debugging + ateam_geometry::Vector velocity(robot_state_msg.twist.linear.x, robot_state_msg.twist.linear.y); + CGAL::Aff_transformation_2 transformation(CGAL::ROTATION, + std::sin(-obj.theta), std::cos(-obj.theta)); + const auto velocity_trans = velocity.transform(transformation); + robot_state_msg.twist_body.linear.x = velocity_trans.x(); + robot_state_msg.twist_body.linear.y = velocity_trans.y(); + robot_state_msg.twist_body.angular.z = robot_state_msg.twist.angular.z; + --health; + } return robot_state_msg; }; int FilteredRobot::getId() const { return bot_id; +} + +bool FilteredRobot::isHealthy(){ + return health > 0; } \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 98501585b..a2766f97f 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -39,6 +39,8 @@ class FilteredRobot { int getId() const; + bool isHealthy() const; + private: int bot_id; ateam_common::TeamColor team; diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 2fa11c28a..7096051df 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -191,13 +191,49 @@ class VisionFilterNode : public rclcpp::Node } void timer_callback() { - // Get the current predictions for all robots and ball(s) - // Make sure these specific critera... - // Should be recently updated enough - prune stale robots/balls - // Pick the best ball if multiple exist (based on whichever is closest to our most recent - // prediction?) - // - // Publish the updated vision info + // Remove the ball if it's bad quality + if (ball.has_value){ + if (!ball.isHealthy()){ + ball.reset(); + } + } + // Erase any robots that are bad quality + std::erase_if(blue_robots, [](FilteredRobot &bot) { return !x.isHealthy()}) + std::erase_if(yellow_robots, [](FilteredRobot &bot) { return !x.isHealthy()}) + // Publish the most recent ball + ateam_msgs::msg::VisionStateBall ball_msg{}; + if (ball.has_value()){ + ball_msg = ball.toMsg(); + } + ball_publisher_->publish(ball_msg); + + // Publish the robots + // - Blue - + for (std::size_t id = 0; id < 16; id++) { + auto robot_msg = ateam_msgs::msg::VisionStateRobot{}; + auto it = std::find_if( + blue_robots.begin(), + blue_robots.end(), + [](){ return id == bot.getId(); } + ); + if (it != blue_robots.end()){ + robot_msg = it->toMsg(); + } + blue_robots_publisher_.at(id)->publish(robot_msg); + } + // - Yellow - + for (std::size_t id = 0; id < 16; id++) { + auto robot_msg = ateam_msgs::msg::VisionStateRobot{}; + auto it = std::find_if( + yellow_robots.begin(), + yellow_robots.end(), + [](){ return id == bot.getId(); } + ); + if (it != yellow_robots.end()){ + robot_msg = it->toMsg(); + } + yellow_robots_publisher_.at(id)->publish(robot_msg); + } return; } From ae8378a25e68adf469eaaaad255ab4808552a2f6 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 15 Feb 2026 22:47:55 -0500 Subject: [PATCH 24/35] It compilesgit add new_super_vision_filter/ YAYgit add new_super_vision_filter/! --- new_super_vision_filter/CMakeLists.txt | 1 + new_super_vision_filter/src/camera.cpp | 50 +------------------ new_super_vision_filter/src/camera.hpp | 10 ---- new_super_vision_filter/src/filter_types.hpp | 6 +++ new_super_vision_filter/src/filtered_ball.cpp | 22 ++++---- new_super_vision_filter/src/filtered_ball.hpp | 12 +++-- .../src/filtered_robot.cpp | 42 ++++++++-------- .../src/filtered_robot.hpp | 10 ++-- .../src/measurements/ball_track.hpp | 4 +- .../src/measurements/robot_track.hpp | 48 +++++++++++------- .../src/vision_filter_node.cpp | 41 +++++++++------ new_super_vision_filter/test/filter_test.cpp | 4 -- 12 files changed, 113 insertions(+), 137 deletions(-) diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt index 5152e66ea..146f22b70 100644 --- a/new_super_vision_filter/CMakeLists.txt +++ b/new_super_vision_filter/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} SHARED src/vision_filter_node.cpp src/camera.cpp src/filtered_robot.cpp + src/filtered_ball.cpp ) target_include_directories(${PROJECT_NAME} diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp index 701e1f171..b73f7cc7c 100644 --- a/new_super_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -30,56 +30,10 @@ Camera::Camera(int camera_id) : camera_id(camera_id) {}; // Have a queue/buffer that we can remove old frames/have a set capacity void Camera::process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg){ - process_balls(detection_frame_msg); - process_robots(detection_frame_msg); + // process_balls(detection_frame_msg); + // process_robots(detection_frame_msg); } void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData &geometry){}; void Camera::clear_old_messages(){}; - -void Camera::process_balls(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg){ - // Geometry msgs stuff... - // https://docs.ros2.org/foxy/api/geometry_msgs/index-msg.html - for (auto ball : detection_frame_msg.balls) { - // For all of our balls, see if we think this is close to an existing measurement - // If not, create a new one - tracked_balls.push_back(FilteredBall(ball)); - } - return; -}; - -void Camera::process_robots(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg){ - for (const auto &robot_detection : detection_frame_msg.robots_blue){ - // Check if this is close to an existing measurement (from an id that we have?) - auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), - [&robot_detection](const FilteredRobot& bot) { - return bot.getId() == robot_detection.robot_id; - }); - - if (existing_bot != tracked_robots.end()){ - existing_bot->update(robot_detection); - } else { - // If not, create a new one - tracked_robots.push_back(FilteredRobot(robot_detection, ateam_common::TeamColor::Blue)); - } - } - for (const auto &robot_detection : detection_frame_msg.robots_yellow){ - // Check if this is close to an existing measurement (from an id that we have?) - auto existing_bot = std::find_if(tracked_robots.begin(), tracked_robots.end(), - [&robot_detection](const RobotTrack& track) { - return track.bot_id == robot_detection.robot_id; - }); - - if (existing_bot != tracked_robots.end()){ - existing_bot->update(robot_detection); - } else { - // If not, create a new one - tracked_robots.push_back(FilteredRobot(robot_detection, ateam_common::TeamColor::Yellow)); - } - } -} - -void Camera::create_new_ball_track(){}; - -void Camera::create_new_robot_track(){}; diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp index ada75be45..b7d4a4bc8 100644 --- a/new_super_vision_filter/src/camera.hpp +++ b/new_super_vision_filter/src/camera.hpp @@ -43,19 +43,9 @@ class Camera { void clear_old_messages(); - void process_balls(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg); - - void process_robots(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg); - - void create_new_ball_track(); - - void create_new_robot_track(); - private: int camera_id; std::chrono::time_point last_updated; - // std::vector tracked_balls; - // std::vector tracked_robots; }; #endif // CAMERA_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp index 3ae7fe411..214ca62dd 100644 --- a/new_super_vision_filter/src/filter_types.hpp +++ b/new_super_vision_filter/src/filter_types.hpp @@ -167,6 +167,12 @@ class AngleMeasurement : public Kalman::Vector { public: KALMAN_VECTOR(AngleMeasurement, double, 1) + + static constexpr size_t W = 0; + + double w() const { + return (*this)[W]; + } }; /* diff --git a/new_super_vision_filter/src/filtered_ball.cpp b/new_super_vision_filter/src/filtered_ball.cpp index 6c1d8f1e2..d252c7a02 100644 --- a/new_super_vision_filter/src/filtered_ball.cpp +++ b/new_super_vision_filter/src/filtered_ball.cpp @@ -1,10 +1,10 @@ #include "filtered_ball.hpp" -FilteredBall::FilteredBall(ssl_league_msgs::msg::VisionDetectionBall &ball_detection_msg){ +FilteredBall::FilteredBall(const BallTrack &track){ PosState initial_state_xy; initial_state_xy << - track.pos.px(), - track.pos.py(), + track.pos.x(), + track.pos.y(), 0, 0; posFilterXY.init(initial_state_xy); @@ -19,7 +19,7 @@ FilteredBall::FilteredBall(ssl_league_msgs::msg::VisionDetectionBall &ball_detec posFilterXY.setCovariance(xy_covariance); } -void FilteredBall::update(BallTrack &track){ +void FilteredBall::update(const BallTrack &track){ // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) if (age < oldEnough) { @@ -30,8 +30,8 @@ void FilteredBall::update(BallTrack &track){ } bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter - const std::chrono::time_point now = - std::chrono::system_clock::now(); + const std::chrono::time_point now = + std::chrono::steady_clock::now(); // If it's been too long, don't use this message if (now - track.timestamp > update_threshold || is_new) { return; @@ -55,15 +55,15 @@ ateam_msgs::msg::VisionStateBall FilteredBall::toMsg(){ if (health > 0 && !is_new) { // NOTE: Does not contain acceleration info ball_state_msg.visible = true; - ball_detection_msg.pose.position.x = posXYEstimate.px(); - ball_detection_msg.pose.position.y = posXYEstimate.py(); - ball_detection_msg.pose.twist.linear.x = posXYEstimate.vx(); - ball_detection_msg.pose.twist.linear.y = posXYEstimate.vy(); + ball_state_msg.pose.position.x = posXYEstimate.px(); + ball_state_msg.pose.position.y = posXYEstimate.py(); + ball_state_msg.twist.linear.x = posXYEstimate.vx(); + ball_state_msg.twist.linear.y = posXYEstimate.vy(); --health; } return ball_state_msg; } -bool FilteredBall::isHealthy(){ +bool FilteredBall::isHealthy() const { return health > 0; } \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp index 782bac4b0..f5d608efb 100644 --- a/new_super_vision_filter/src/filtered_ball.hpp +++ b/new_super_vision_filter/src/filtered_ball.hpp @@ -24,9 +24,10 @@ #include #include #include -#include +#include #include "kalman/ExtendedKalmanFilter.hpp" #include "filter_types.hpp" +#include "measurements/ball_track.hpp" enum KickState { ROLLING, @@ -36,11 +37,11 @@ enum KickState { class FilteredBall { public: - FilteredBall(ssl_league_msgs::msg::VisionDetectionBall &ball_detection_msg); + FilteredBall(const BallTrack &track); - void update(ssl_league_msgs::msg::VisionDetectionBall &ball_detection) + void update(const BallTrack &track); - ateam_msgs::msg::BallState toMsg(); + ateam_msgs::msg::VisionStateBall toMsg(); bool isHealthy() const; @@ -56,6 +57,9 @@ class FilteredBall { std::chrono::steady_clock::time_point last_visible_timestamp; // Filter Kalman::ExtendedKalmanFilter posFilterXY; + PosState posXYEstimate; + PosSystemModel systemModelXY; + PosMeasurementModel measurementModelXY; }; #endif // FILTERED_BALL_HPP_ diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index ba3a85318..40bb47162 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -31,13 +31,13 @@ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ // OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp -FilteredRobot::FilteredRobot(RobotTrack &track, ateam_common::TeamColor team_color) - : posFilterXY(), posFilterW(), bot_id(track.robot_id), team(team_color) { +FilteredRobot::FilteredRobot(const RobotTrack &track, ateam_common::TeamColor team_color) + : posFilterXY(), posFilterW(), bot_id(track.getId()), team(team_color) { // Initialize XY KF PosState initial_state_xy; initial_state_xy << - track.pos.px(), - track.pos.py(), + track.pos.x(), + track.pos.y(), 0, 0; posFilterXY.init(initial_state_xy); @@ -59,7 +59,7 @@ FilteredRobot::FilteredRobot(RobotTrack &track, ateam_common::TeamColor team_col */ AngleState initial_state_w; initial_state_w << - track.angle.pw(), + track.angle.w(), 0; posFilterW.init(initial_state_w); /* @@ -72,7 +72,7 @@ FilteredRobot::FilteredRobot(RobotTrack &track, ateam_common::TeamColor team_col posFilterW.setCovariance(w_covariance); } -void FilteredRobot::update(RobotTrack &track) { +void FilteredRobot::update(const RobotTrack &track) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) if (age < oldEnough) { @@ -83,10 +83,10 @@ void FilteredRobot::update(RobotTrack &track) { } bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter - const std::chrono::time_point now = - std::chrono::system_clock::now(); + const std::chrono::time_point now = + std::chrono::steady_clock::now(); // If it's been too long, don't use this message - if (now - track.timestamp > update_threshold || is_new) { + if (now - track.getTimestamp() > update_threshold || is_new) { return; } // Predict state forward @@ -103,27 +103,27 @@ void FilteredRobot::update(RobotTrack &track) { posWEstimate = posFilterW.update(measurementModelW, track.angle); } -ateam_msgs::msg::RobotState FilteredRobot::toMsg(){ +ateam_msgs::msg::VisionStateRobot FilteredRobot::toMsg(){ ateam_msgs::msg::VisionStateRobot robot_state_msg{}; bool is_new = age < oldEnough; if (health > 0 && !is_new) { robot_state_msg.visible = true; - robot_detection_msg.pose.position.x = posXYEstimate.px(); - robot_detection_msg.pose.position.y = posXYEstimate.py(); - robot_detection_msg.pose.twist.linear.x = posXYEstimate.vx(); - robot_detection_msg.pose.twist.linear.y = posXYEstimate.vy(); + robot_state_msg.pose.position.x = posXYEstimate.px(); + robot_state_msg.pose.position.y = posXYEstimate.py(); + robot_state_msg.twist.linear.x = posXYEstimate.vx(); + robot_state_msg.twist.linear.y = posXYEstimate.vy(); - robot_detection_msg.pose.orientation.x = 0; - robot_detection_msg.pose.orientation.y = 0; - robot_detection_msg.pose.orientation.z = 1; - robot_detection_msg.pose.orientation.w = posWEstimate.pw(); - robot_detection_msg.twist.angular.z = posWEstimate.vw(); + robot_state_msg.pose.orientation.x = 0; + robot_state_msg.pose.orientation.y = 0; + robot_state_msg.pose.orientation.z = 1; + robot_state_msg.pose.orientation.w = posWEstimate.pw(); + robot_state_msg.twist.angular.z = posWEstimate.vw(); // Convert to body velocities for plotting/debugging ateam_geometry::Vector velocity(robot_state_msg.twist.linear.x, robot_state_msg.twist.linear.y); CGAL::Aff_transformation_2 transformation(CGAL::ROTATION, - std::sin(-obj.theta), std::cos(-obj.theta)); + std::sin(-posWEstimate.pw()), std::cos(-posWEstimate.pw())); const auto velocity_trans = velocity.transform(transformation); robot_state_msg.twist_body.linear.x = velocity_trans.x(); robot_state_msg.twist_body.linear.y = velocity_trans.y(); @@ -137,6 +137,6 @@ int FilteredRobot::getId() const { return bot_id; } -bool FilteredRobot::isHealthy(){ +bool FilteredRobot::isHealthy() const { return health > 0; } \ No newline at end of file diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index a2766f97f..62582c7a2 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -22,6 +22,7 @@ #include "kalman/ExtendedKalmanFilter.hpp" #include "filter_types.hpp" +#include "measurements/robot_track.hpp" #include #include @@ -31,11 +32,11 @@ class FilteredRobot { public: - FilteredRobot(RobotTrack &track, ateam_common::TeamColor team_color); + FilteredRobot(const RobotTrack &track, ateam_common::TeamColor team_color); - void update(ssl_league_msgs::msg::VisionDetectionRobot robot_detection); + void update(const RobotTrack &track); - ateam_msgs::msg::RobotState toMsg(); + ateam_msgs::msg::VisionStateRobot toMsg(); int getId() const; @@ -46,6 +47,7 @@ class FilteredRobot { ateam_common::TeamColor team; int age = 0; int oldEnough = 3; + int health = 2; int maxHealth = 20; double maxDistance = -1.0; std::chrono::milliseconds update_threshold{50}; @@ -58,10 +60,12 @@ class FilteredRobot { Kalman::ExtendedKalmanFilter posFilterXY; PosSystemModel systemModelXY; PosState posXYEstimate{}; + PosMeasurementModel measurementModelXY; // Theta Kalman::ExtendedKalmanFilter posFilterW; AngleSystemModel systemModelW; AngleState posWEstimate{}; + AngleMeasurementModel measurementModelW; }; #endif // FILTERED_ROBOT_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/measurements/ball_track.hpp b/new_super_vision_filter/src/measurements/ball_track.hpp index b9aad1436..ed22302a2 100644 --- a/new_super_vision_filter/src/measurements/ball_track.hpp +++ b/new_super_vision_filter/src/measurements/ball_track.hpp @@ -27,7 +27,7 @@ class BallTrack { public: - BallTrack(ssl_league_msgs::msg:VisionDetectionBall &ball_detection, int &camera_id) : camera_id(camera_id) { + BallTrack(const ssl_league_msgs::msg::VisionDetectionBall &ball_detection, int &camera_id) : camera_id(camera_id) { PosMeasurement pos; pos << ball_detection.pos.x, ball_detection.pos.y; @@ -37,6 +37,6 @@ class BallTrack { PosMeasurement pos; std::chrono::time_point timestamp; int camera_id; -} +}; #endif // BALL_TRACK_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/measurements/robot_track.hpp b/new_super_vision_filter/src/measurements/robot_track.hpp index a2cba1b02..6c350da41 100644 --- a/new_super_vision_filter/src/measurements/robot_track.hpp +++ b/new_super_vision_filter/src/measurements/robot_track.hpp @@ -31,24 +31,36 @@ * used to provide updates to the Kalman filter. */ class RobotTrack { - RobotTrack( - ssl_league_msgs::msg::VisionDetectionRobot &bot_detection, - int &camera_id, - ateam_common::TeamColor &team - ) : camera_id(camera_id), team(team) { + public: + RobotTrack( + const ssl_league_msgs::msg::VisionDetectionRobot &bot_detection, + int &camera_id, + ateam_common::TeamColor &team + ) : camera_id(camera_id), team(team) { + PosMeasurement pos; + pos << bot_detection.pose.position.x, + bot_detection.pose.position.y; + angle << bot_detection.pose.orientation.w; + robot_id = bot_detection.robot_id; + } + + int getId() const { + return robot_id; + }; + + std::chrono::time_point getTimestamp() const { + return timestamp; + } + PosMeasurement pos; - pos << bot_detection.pose.position.x, - bot_detection.pose.position.y; - angle << bot_detection.pose.orientation.w; - robot_id = bot_detection.robot_id; - } - - PosMeasurement pos; - AngleMeasurement angle; - std::chrono::time_point timestamp; - int camera_id; - ateam_common::TeamColor team; - int robot_id; -} + AngleMeasurement angle; + + private: + std::chrono::time_point timestamp; + int camera_id; + ateam_common::TeamColor team; + int robot_id; + +}; #endif // ROBOT_TRACK_HPP_ \ No newline at end of file diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 7096051df..82948bbd6 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -24,12 +24,17 @@ #include "measurements/robot_track.hpp" #include +#include #include +#include #include +#include #include #include #include +using namespace std::chrono_literals; + namespace new_super_vision { class VisionFilterNode : public rclcpp::Node @@ -107,10 +112,10 @@ class VisionFilterNode : public rclcpp::Node 16> blue_robots_publisher_; std::array::SharedPtr, 16> yellow_robots_publisher_; + rclcpp::Publisher::SharedPtr ball_publisher_; ateam_common::GameControllerListener game_controller_listener_; rclcpp::Subscription::SharedPtr ssl_vision_subscription_; - rclcpp::Publisher::SharedPtr vision_state_publisher_; // The two below are in case we are sharing a field during testing at event rclcpp::Subscription::SharedPtr field_subscription_; int ignore_side_; @@ -126,15 +131,17 @@ class VisionFilterNode : public rclcpp::Node } // Create a track from each robot in this message - for (const auto& bot : detection.yellow_robots) { + for (const auto& bot : detection.robots_yellow) { + ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow; yellow_tracks.push_back( - RobotTrack(bot, detect_camera, ateam_common::TeamColor::Yellow) + RobotTrack(bot, detect_camera, team_color) ); } - for (const auto& bot : detection.blue_robots) { + for (const auto& bot : detection.robots_blue) { + ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; blue_tracks.push_back( - RobotTrack(bot, detect_camera, ateam_common::TeamColor::Blue) + RobotTrack(bot, detect_camera, team_color) ); } @@ -151,28 +158,30 @@ class VisionFilterNode : public rclcpp::Node // Process all the new updates from the tracks for (const auto& bot_track : blue_tracks){ + ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; auto it = std::find_if( blue_robots.begin(), blue_robots.end(), - [](){ return bot_track.robot_id == bot.getId(); } + [bot_track](const FilteredRobot &bot){ return bot_track.getId() == bot.getId(); } ); if (it != blue_robots.end()){ blue_robots.push_back( - FilteredRobot(bot_track, ateam_common::TeamColor::Blue) + FilteredRobot(bot_track, team_color) ); } else { it->update(bot_track); } } for (const auto& bot_track : yellow_tracks){ + ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow; auto it = std::find_if( yellow_robots.begin(), yellow_robots.end(), - [](){ return bot_track.robot_id == bot.getId(); } + [bot_track](const FilteredRobot &bot){ return bot_track.getId() == bot.getId(); } ); if (it != yellow_robots.end()){ yellow_robots.push_back( - FilteredRobot(bot_track, ateam_common::TeamColor::Yellow) + FilteredRobot(bot_track, team_color) ); } else { it->update(bot_track); @@ -192,18 +201,18 @@ class VisionFilterNode : public rclcpp::Node void timer_callback() { // Remove the ball if it's bad quality - if (ball.has_value){ - if (!ball.isHealthy()){ + if (ball.has_value()){ + if (!ball.value().isHealthy()){ ball.reset(); } } // Erase any robots that are bad quality - std::erase_if(blue_robots, [](FilteredRobot &bot) { return !x.isHealthy()}) - std::erase_if(yellow_robots, [](FilteredRobot &bot) { return !x.isHealthy()}) + std::erase_if(blue_robots, [](FilteredRobot &bot) { return !bot.isHealthy();}); + std::erase_if(yellow_robots, [](FilteredRobot &bot) { return !bot.isHealthy();}); // Publish the most recent ball ateam_msgs::msg::VisionStateBall ball_msg{}; if (ball.has_value()){ - ball_msg = ball.toMsg(); + ball_msg = ball.value().toMsg(); } ball_publisher_->publish(ball_msg); @@ -214,7 +223,7 @@ class VisionFilterNode : public rclcpp::Node auto it = std::find_if( blue_robots.begin(), blue_robots.end(), - [](){ return id == bot.getId(); } + [id](const FilteredRobot &bot){ return id == bot.getId(); } ); if (it != blue_robots.end()){ robot_msg = it->toMsg(); @@ -227,7 +236,7 @@ class VisionFilterNode : public rclcpp::Node auto it = std::find_if( yellow_robots.begin(), yellow_robots.end(), - [](){ return id == bot.getId(); } + [id](const FilteredRobot &bot){ return id == bot.getId(); } ); if (it != yellow_robots.end()){ robot_msg = it->toMsg(); diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp index 7ae86e72b..4051b89a6 100644 --- a/new_super_vision_filter/test/filter_test.cpp +++ b/new_super_vision_filter/test/filter_test.cpp @@ -32,10 +32,6 @@ class FilteredRobotTest : public ::testing::Test { ssl_league_msgs::msg::VisionDetectionRobot robot_msg{}; auto team = ateam_common::TeamColor::Blue; - FilteredRobot robot{ - robot_msg, - team - }; } }; From 491b22c7fab13ba205a6398bdeee7149a8871c4e Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 15 Feb 2026 22:55:09 -0500 Subject: [PATCH 25/35] uncrustify --- new_super_vision_filter/src/camera.cpp | 14 +- new_super_vision_filter/src/camera.hpp | 23 +- new_super_vision_filter/src/filter_types.hpp | 195 +++++------ new_super_vision_filter/src/filtered_ball.cpp | 92 +++--- new_super_vision_filter/src/filtered_ball.hpp | 49 +-- .../src/filtered_robot.cpp | 149 ++++----- .../src/filtered_robot.hpp | 56 ++-- new_super_vision_filter/src/frame_info.hpp | 11 - .../src/measurements/ball_track.hpp | 24 +- .../src/measurements/robot_track.hpp | 60 ++-- .../src/vision_filter_node.cpp | 304 +++++++++--------- new_super_vision_filter/test/filter_test.cpp | 24 +- 12 files changed, 508 insertions(+), 493 deletions(-) delete mode 100644 new_super_vision_filter/src/frame_info.hpp diff --git a/new_super_vision_filter/src/camera.cpp b/new_super_vision_filter/src/camera.cpp index b73f7cc7c..a2e0c5f86 100644 --- a/new_super_vision_filter/src/camera.cpp +++ b/new_super_vision_filter/src/camera.cpp @@ -24,16 +24,16 @@ #include "filtered_ball.hpp" #include "filtered_robot.hpp" -Camera::Camera(int camera_id) : camera_id(camera_id) {}; -// Need to process an individual frame -// Set geometry from VisionGeometryCameraCalibration.msg -// Have a queue/buffer that we can remove old frames/have a set capacity +Camera::Camera(int camera_id) +: camera_id(camera_id) {} -void Camera::process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg){ +void Camera::process_detection_frame( + const ssl_league_msgs::msg::VisionDetectionFrame & detection_frame_msg) +{ // process_balls(detection_frame_msg); // process_robots(detection_frame_msg); } -void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData &geometry){}; +void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData & geometry) {} -void Camera::clear_old_messages(){}; +void Camera::clear_old_messages() {} diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp index b7d4a4bc8..132c4f0f6 100644 --- a/new_super_vision_filter/src/camera.hpp +++ b/new_super_vision_filter/src/camera.hpp @@ -18,8 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#ifndef CAMERA_HPP_ -#define CAMERA_HPP_ +#ifndef CAMERA_HPP_ +#define CAMERA_HPP_ #include #include @@ -31,21 +31,22 @@ #include "filtered_robot.hpp" class Camera { - public: +public: // Need to process an individual frame // Set geometry from VisionGeometryCameraCalibration.msg // Have a queue/buffer that we can remove old frames/have a set capacity - Camera(int camera_id); + Camera(int camera_id); - void process_detection_frame(const ssl_league_msgs::msg::VisionDetectionFrame &detection_frame_msg); + void process_detection_frame( + const ssl_league_msgs::msg::VisionDetectionFrame & detection_frame_msg); - void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData &geometry); + void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData & geometry); - void clear_old_messages(); + void clear_old_messages(); - private: - int camera_id; - std::chrono::time_point last_updated; +private: + int camera_id; + std::chrono::time_point last_updated; }; -#endif // CAMERA_HPP_ \ No newline at end of file +#endif // CAMERA_HPP_ diff --git a/new_super_vision_filter/src/filter_types.hpp b/new_super_vision_filter/src/filter_types.hpp index 214ca62dd..7d58a40b4 100644 --- a/new_super_vision_filter/src/filter_types.hpp +++ b/new_super_vision_filter/src/filter_types.hpp @@ -18,8 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#ifndef FILTER_TYPES_HPP_ -#define FILTER_TYPES_HPP_ +#ifndef FILTER_TYPES_HPP_ +#define FILTER_TYPES_HPP_ #include @@ -40,45 +40,47 @@ class PosMeasurement : public Kalman::Vector { public: - KALMAN_VECTOR(PosMeasurement, double, 2) - - static constexpr size_t X = 0; - static constexpr size_t Y = 1; + KALMAN_VECTOR(PosMeasurement, double, 2) - double x() const { - return (*this)[X]; - } - - double y() const { - return (*this)[Y]; - } + static constexpr size_t X = 0; + static constexpr size_t Y = 1; + + double x() const + { + return (*this)[X]; + } + + double y() const + { + return (*this)[Y]; + } }; /* 4D state vector for robot or ball position - - State vector structure is + + State vector structure is { x_pos, y_pos, x_vel, y_vel } - + Measurements are in m */ class PosState : public Kalman::Vector { public: - KALMAN_VECTOR(PosState, double, 4) + KALMAN_VECTOR(PosState, double, 4) - static constexpr size_t PX = 0; - static constexpr size_t PY = 1; - static constexpr size_t VX = 2; - static constexpr size_t VY = 3; + static constexpr size_t PX = 0; + static constexpr size_t PY = 1; + static constexpr size_t VX = 2; + static constexpr size_t VY = 3; - const double px(){return (*this)[PX]; } - const double py(){return (*this)[PY]; } - const double vx(){return (*this)[VX]; } - const double vy(){return (*this)[VY]; } + const double px() {return (*this)[PX];} + const double py() {return (*this)[PY];} + const double vx() {return (*this)[VX];} + const double vy() {return (*this)[VY];} }; @@ -89,27 +91,28 @@ class PosState : public Kalman::Vector Curently assumes no measurement noise */ class PosMeasurementModel - : public Kalman::LinearizedMeasurementModel + : public Kalman::LinearizedMeasurementModel { public: // h(x) = predicted measurement - PosMeasurement h(const PosState& x) const - { - PosMeasurement z; - z[0] = x(x.PX); // px - z[1] = x(x.PY); // py - return z; - } + PosMeasurement h(const PosState & x) const + { + PosMeasurement z; + z[0] = x(x.PX); // px + z[1] = x(x.PY); // py + return z; + } // Jacobian H = ∂h/∂x + protected: - void updateJacobians(const PosState& x) - { - this->H.setZero(); + void updateJacobians(const PosState & x) + { + this->H.setZero(); - this->H(0, 0) = 1; // dz_px / d_px - this->H(1, 1) = 1; // dz_py / d_py - } + this->H(0, 0) = 1; // dz_px / d_px + this->H(1, 1) = 1; // dz_py / d_py + } }; /* @@ -119,15 +122,15 @@ class PosMeasurementModel */ class PosSystemModel : public Kalman::LinearizedSystemModel { - private: - mutable std::chrono::time_point last_update; +private: + mutable std::chrono::time_point last_update; - public: +public: // For now, we don't add any control inputs to the vision system // TODO (Christian): We might need this for the robots, even if we // don't use it for the ball - using Control = Kalman::Vector; - using Seconds = std::chrono::duration; + using Control = Kalman::Vector; + using Seconds = std::chrono::duration; /* The f() function applies what would be the A (state transition) @@ -139,24 +142,24 @@ class PosSystemModel : public Kalman::LinearizedSystemModel pos_t = pos_{t-1} + vel_{t-1} * dt vel_t = vel_{t-1} */ - PosState f(const PosState& x, const Control& /*u*/) const override - { - PosState x_updated{}; - auto now = std::chrono::steady_clock::now(); - Seconds dt = now - last_update; - double dt_s = dt.count(); - + PosState f(const PosState & x, const Control & /*u*/) const override + { + PosState x_updated{}; + auto now = std::chrono::steady_clock::now(); + Seconds dt = now - last_update; + double dt_s = dt.count(); + // B/c dt is in ms, we need to convert to s, since // our velocities are all in m/s - x_updated(x.PX) = x(x.PX) + x(x.VX) * dt_s; + x_updated(x.PX) = x(x.PX) + x(x.VX) * dt_s; // We assume constant velocity in the system model - x_updated(x.VX) = x(x.VX); - x_updated(x.PY) = x(x.PY) + x(x.VY) * dt_s; - x_updated(x.VY) = x(x.VY); + x_updated(x.VX) = x(x.VX); + x_updated(x.PY) = x(x.PY) + x(x.VY) * dt_s; + x_updated(x.VY) = x(x.VY); - last_update = now; - return x_updated; - } + last_update = now; + return x_updated; + } }; @@ -166,13 +169,14 @@ class PosSystemModel : public Kalman::LinearizedSystemModel class AngleMeasurement : public Kalman::Vector { public: - KALMAN_VECTOR(AngleMeasurement, double, 1) + KALMAN_VECTOR(AngleMeasurement, double, 1) - static constexpr size_t W = 0; + static constexpr size_t W = 0; - double w() const { - return (*this)[W]; - } + double w() const + { + return (*this)[W]; + } }; /* @@ -187,13 +191,13 @@ class AngleMeasurement : public Kalman::Vector class AngleState : public Kalman::Vector { public: - KALMAN_VECTOR(AngleState, double, 2) + KALMAN_VECTOR(AngleState, double, 2) + + static constexpr size_t PW = 0; + static constexpr size_t VW = 1; - static constexpr size_t PW = 0; - static constexpr size_t VW = 1; - - const double pw(){return (*this)[PW]; } - const double vw(){return (*this)[VW]; } + const double pw() {return (*this)[PW];} + const double vw() {return (*this)[VW];} }; /* @@ -202,33 +206,33 @@ class AngleState : public Kalman::Vector Currently assumes no measurement noise */ class AngleMeasurementModel - : public Kalman::LinearizedMeasurementModel + : public Kalman::LinearizedMeasurementModel { public: // h(x) = predicted measurement - AngleMeasurement h(const AngleState& x) const override - { - AngleMeasurement z{}; - z[0] = x[0]; // pw - return z; - } + AngleMeasurement h(const AngleState & x) const override + { + AngleMeasurement z{}; + z[0] = x[0]; // pw + return z; + } // Jacobian H = ∂h/∂x - void updateJacobians(const AngleState& x) override - { - this->H.setZero(); - this->H(0, 0) = 1; // dz_px / d_px - } + void updateJacobians(const AngleState & x) override + { + this->H.setZero(); + this->H(0, 0) = 1; // dz_px / d_px + } }; class AngleSystemModel : public Kalman::LinearizedSystemModel { - private: - mutable std::chrono::time_point last_update; +private: + mutable std::chrono::time_point last_update; - public: - using Control = Kalman::Vector; - using Seconds = std::chrono::duration; +public: + using Control = Kalman::Vector; + using Seconds = std::chrono::duration; /* This is the system's state transition function (applies the A matrix) @@ -239,17 +243,18 @@ class AngleSystemModel : public Kalman::LinearizedSystemModel pos_t = pos_{t-1} + vel_{t-1} * dt vel_t = vel_{t-1} */ - AngleState f(const AngleState& x, const Control& ) const { - AngleState x_updated{}; + AngleState f(const AngleState & x, const Control &) const + { + AngleState x_updated{}; - const auto now = std::chrono::steady_clock::now(); - Seconds dt = now - last_update; - double dt_s = dt.count(); + const auto now = std::chrono::steady_clock::now(); + Seconds dt = now - last_update; + double dt_s = dt.count(); - x_updated(x.PW) = x(x.PW) + x(x.VW) * dt_s; + x_updated(x.PW) = x(x.PW) + x(x.VW) * dt_s; - return x_updated; - } + return x_updated; + } }; -#endif // FILTER_TYPES_HPP \ No newline at end of file +#endif // FILTER_TYPES_HPP diff --git a/new_super_vision_filter/src/filtered_ball.cpp b/new_super_vision_filter/src/filtered_ball.cpp index d252c7a02..f6b7b0238 100644 --- a/new_super_vision_filter/src/filtered_ball.cpp +++ b/new_super_vision_filter/src/filtered_ball.cpp @@ -1,69 +1,73 @@ #include "filtered_ball.hpp" -FilteredBall::FilteredBall(const BallTrack &track){ - PosState initial_state_xy; - initial_state_xy << - track.pos.x(), - track.pos.y(), - 0, - 0; - posFilterXY.init(initial_state_xy); - Kalman::Matrix xy_covariance; +FilteredBall::FilteredBall(const BallTrack & track) +{ + PosState initial_state_xy; + initial_state_xy << + track.pos.x(), + track.pos.y(), + 0, + 0; + posFilterXY.init(initial_state_xy); + Kalman::Matrix xy_covariance; // This is in m, so initial covariance is 100 mm. // We don't get a velocity input in the measurement itself, // so that has a large initial uncertainty. - xy_covariance << 1e-2, 0, 0, 0, - 0, 1e-2, 0, 0, - 0, 0, 1e3, 0, - 0, 0, 0, 1e3; - posFilterXY.setCovariance(xy_covariance); + xy_covariance << 1e-2, 0, 0, 0, + 0, 1e-2, 0, 0, + 0, 0, 1e3, 0, + 0, 0, 0, 1e3; + posFilterXY.setCovariance(xy_covariance); } -void FilteredBall::update(const BallTrack &track){ +void FilteredBall::update(const BallTrack & track) +{ // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) - if (age < oldEnough) { - ++age; - } - if (health < maxHealth){ - health += 2; - } - bool is_new = age < oldEnough; + if (age < oldEnough) { + ++age; + } + if (health < maxHealth) { + health += 2; + } + bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter - const std::chrono::time_point now = - std::chrono::steady_clock::now(); + const std::chrono::time_point now = + std::chrono::steady_clock::now(); // If it's been too long, don't use this message - if (now - track.timestamp > update_threshold || is_new) { - return; - } + if (now - track.timestamp > update_threshold || is_new) { + return; + } // Predict state forward // Predict covariance forward // (All encompassed by the .predict() function) - auto xy_pred = posFilterXY.predict(systemModelXY); + auto xy_pred = posFilterXY.predict(systemModelXY); // Update the Jacobian matrix (contained in filter) // Compute Kalman gain (contained in filter) // Update state estimate (returned) // Update covariance estimate (contained in filter) // All encompassed by the .update() function - posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); + posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); } -ateam_msgs::msg::VisionStateBall FilteredBall::toMsg(){ - ateam_msgs::msg::VisionStateBall ball_state_msg{}; - bool is_new = age < oldEnough; +ateam_msgs::msg::VisionStateBall FilteredBall::toMsg() +{ + ateam_msgs::msg::VisionStateBall ball_state_msg{}; + bool is_new = age < oldEnough; - if (health > 0 && !is_new) { + if (health > 0 && !is_new) { // NOTE: Does not contain acceleration info - ball_state_msg.visible = true; - ball_state_msg.pose.position.x = posXYEstimate.px(); - ball_state_msg.pose.position.y = posXYEstimate.py(); - ball_state_msg.twist.linear.x = posXYEstimate.vx(); - ball_state_msg.twist.linear.y = posXYEstimate.vy(); - --health; - } - return ball_state_msg; + ball_state_msg.visible = true; + ball_state_msg.pose.position.x = posXYEstimate.px(); + ball_state_msg.pose.position.y = posXYEstimate.py(); + ball_state_msg.twist.linear.x = posXYEstimate.vx(); + ball_state_msg.twist.linear.y = posXYEstimate.vy(); + --health; + } + return ball_state_msg; } -bool FilteredBall::isHealthy() const { - return health > 0; -} \ No newline at end of file +bool FilteredBall::isHealthy() const +{ + return health > 0; +} diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp index f5d608efb..df5bf2b96 100644 --- a/new_super_vision_filter/src/filtered_ball.hpp +++ b/new_super_vision_filter/src/filtered_ball.hpp @@ -18,7 +18,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#ifndef FILTERED_BALL_HPP_ +#ifndef FILTERED_BALL_HPP_ #define FILTERED_BALL_HPP_ #include @@ -29,37 +29,38 @@ #include "filter_types.hpp" #include "measurements/ball_track.hpp" -enum KickState { - ROLLING, - KICKED, - CHIPPED +enum KickState +{ + ROLLING, + KICKED, + CHIPPED }; class FilteredBall { - public: - FilteredBall(const BallTrack &track); +public: + FilteredBall(const BallTrack & track); - void update(const BallTrack &track); + void update(const BallTrack & track); - ateam_msgs::msg::VisionStateBall toMsg(); + ateam_msgs::msg::VisionStateBall toMsg(); - bool isHealthy() const; + bool isHealthy() const; - private: - int age = 0; - int oldEnough = 3; - int maxHealth = 20; - int health = 2; - double maxDistance = -1.0; - KickState currentKickState = ROLLING; - std::chrono::milliseconds update_threshold{50}; - std::chrono::steady_clock::time_point timestamp; - std::chrono::steady_clock::time_point last_visible_timestamp; +private: + int age = 0; + int oldEnough = 3; + int maxHealth = 20; + int health = 2; + double maxDistance = -1.0; + KickState currentKickState = ROLLING; + std::chrono::milliseconds update_threshold{50}; + std::chrono::steady_clock::time_point timestamp; + std::chrono::steady_clock::time_point last_visible_timestamp; // Filter - Kalman::ExtendedKalmanFilter posFilterXY; - PosState posXYEstimate; - PosSystemModel systemModelXY; - PosMeasurementModel measurementModelXY; + Kalman::ExtendedKalmanFilter posFilterXY; + PosState posXYEstimate; + PosSystemModel systemModelXY; + PosMeasurementModel measurementModelXY; }; #endif // FILTERED_BALL_HPP_ diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 40bb47162..62af37bdb 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -31,25 +31,26 @@ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ // OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp -FilteredRobot::FilteredRobot(const RobotTrack &track, ateam_common::TeamColor team_color) - : posFilterXY(), posFilterW(), bot_id(track.getId()), team(team_color) { +FilteredRobot::FilteredRobot(const RobotTrack & track, ateam_common::TeamColor team_color) +: posFilterXY(), posFilterW(), bot_id(track.getId()), team(team_color) +{ // Initialize XY KF - PosState initial_state_xy; - initial_state_xy << - track.pos.x(), - track.pos.y(), - 0, - 0; - posFilterXY.init(initial_state_xy); - Kalman::Matrix xy_covariance; + PosState initial_state_xy; + initial_state_xy << + track.pos.x(), + track.pos.y(), + 0, + 0; + posFilterXY.init(initial_state_xy); + Kalman::Matrix xy_covariance; // This is in m, so initial covariance is 100 mm. // We don't get a velocity input in the measurement itself, // so that has a large initial uncertainty. - xy_covariance << 1e-2, 0, 0, 0, - 0, 1e-2, 0, 0, - 0, 0, 1e3, 0, - 0, 0, 0, 1e3; - posFilterXY.setCovariance(xy_covariance); + xy_covariance << 1e-2, 0, 0, 0, + 0, 1e-2, 0, 0, + 0, 0, 1e3, 0, + 0, 0, 0, 1e3; + posFilterXY.setCovariance(xy_covariance); // Initialize angular KF /* @@ -57,86 +58,90 @@ FilteredRobot::FilteredRobot(const RobotTrack &track, ateam_common::TeamColor te w_pos, w_vel */ - AngleState initial_state_w; - initial_state_w << - track.angle.w(), - 0; - posFilterW.init(initial_state_w); + AngleState initial_state_w; + initial_state_w << + track.angle.w(), + 0; + posFilterW.init(initial_state_w); /* Initial covariance is approx 2 deg. for pos, 10 deg. for vel */ - Kalman::Matrix w_covariance; - w_covariance << std::pow(2*M_PI/180.0, 2), 0, - 0, std::pow(10*M_PI/180.0, 2); - posFilterW.setCovariance(w_covariance); - } + Kalman::Matrix w_covariance; + w_covariance << std::pow(2 * M_PI / 180.0, 2), 0, + 0, std::pow(10 * M_PI / 180.0, 2); + posFilterW.setCovariance(w_covariance); +} -void FilteredRobot::update(const RobotTrack &track) { +void FilteredRobot::update(const RobotTrack & track) +{ // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) - if (age < oldEnough) { - ++age; - } - if (health < maxHealth){ - health += 2; - } - bool is_new = age < oldEnough; + if (age < oldEnough) { + ++age; + } + if (health < maxHealth) { + health += 2; + } + bool is_new = age < oldEnough; // As long as its reasonable, update the Kalman Filter - const std::chrono::time_point now = - std::chrono::steady_clock::now(); + const std::chrono::time_point now = + std::chrono::steady_clock::now(); // If it's been too long, don't use this message - if (now - track.getTimestamp() > update_threshold || is_new) { - return; - } + if (now - track.getTimestamp() > update_threshold || is_new) { + return; + } // Predict state forward // Predict covariance forward // (All encompassed by the .predict() function) - auto xy_pred = posFilterXY.predict(systemModelXY); - auto w_pred = posFilterW.predict(systemModelW); + auto xy_pred = posFilterXY.predict(systemModelXY); + auto w_pred = posFilterW.predict(systemModelW); // Update the Jacobian matrix (contained in filter) // Compute Kalman gain (contained in filter) // Update state estimate (returned) // Update covariance estimate (contained in filter) // All encompassed by the .update() function - posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); - posWEstimate = posFilterW.update(measurementModelW, track.angle); + posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); + posWEstimate = posFilterW.update(measurementModelW, track.angle); } -ateam_msgs::msg::VisionStateRobot FilteredRobot::toMsg(){ - ateam_msgs::msg::VisionStateRobot robot_state_msg{}; - bool is_new = age < oldEnough; +ateam_msgs::msg::VisionStateRobot FilteredRobot::toMsg() +{ + ateam_msgs::msg::VisionStateRobot robot_state_msg{}; + bool is_new = age < oldEnough; - if (health > 0 && !is_new) { - robot_state_msg.visible = true; - robot_state_msg.pose.position.x = posXYEstimate.px(); - robot_state_msg.pose.position.y = posXYEstimate.py(); - robot_state_msg.twist.linear.x = posXYEstimate.vx(); - robot_state_msg.twist.linear.y = posXYEstimate.vy(); + if (health > 0 && !is_new) { + robot_state_msg.visible = true; + robot_state_msg.pose.position.x = posXYEstimate.px(); + robot_state_msg.pose.position.y = posXYEstimate.py(); + robot_state_msg.twist.linear.x = posXYEstimate.vx(); + robot_state_msg.twist.linear.y = posXYEstimate.vy(); - robot_state_msg.pose.orientation.x = 0; - robot_state_msg.pose.orientation.y = 0; - robot_state_msg.pose.orientation.z = 1; - robot_state_msg.pose.orientation.w = posWEstimate.pw(); - robot_state_msg.twist.angular.z = posWEstimate.vw(); + robot_state_msg.pose.orientation.x = 0; + robot_state_msg.pose.orientation.y = 0; + robot_state_msg.pose.orientation.z = 1; + robot_state_msg.pose.orientation.w = posWEstimate.pw(); + robot_state_msg.twist.angular.z = posWEstimate.vw(); // Convert to body velocities for plotting/debugging - ateam_geometry::Vector velocity(robot_state_msg.twist.linear.x, robot_state_msg.twist.linear.y); - CGAL::Aff_transformation_2 transformation(CGAL::ROTATION, - std::sin(-posWEstimate.pw()), std::cos(-posWEstimate.pw())); - const auto velocity_trans = velocity.transform(transformation); - robot_state_msg.twist_body.linear.x = velocity_trans.x(); - robot_state_msg.twist_body.linear.y = velocity_trans.y(); - robot_state_msg.twist_body.angular.z = robot_state_msg.twist.angular.z; - --health; - } - return robot_state_msg; -}; + ateam_geometry::Vector velocity(robot_state_msg.twist.linear.x, robot_state_msg.twist.linear.y); + CGAL::Aff_transformation_2 transformation(CGAL::ROTATION, + std::sin(-posWEstimate.pw()), std::cos(-posWEstimate.pw())); + const auto velocity_trans = velocity.transform(transformation); + robot_state_msg.twist_body.linear.x = velocity_trans.x(); + robot_state_msg.twist_body.linear.y = velocity_trans.y(); + robot_state_msg.twist_body.angular.z = robot_state_msg.twist.angular.z; + --health; + } + return robot_state_msg; +} -int FilteredRobot::getId() const { - return bot_id; +int FilteredRobot::getId() const +{ + return bot_id; } -bool FilteredRobot::isHealthy() const { - return health > 0; -} \ No newline at end of file +bool FilteredRobot::isHealthy() const +{ + return health > 0; +} diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 62582c7a2..487f72d89 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -17,8 +17,8 @@ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#ifndef FILTERED_ROBOT_HPP_ -#define FILTERED_ROBOT_HPP_ +#ifndef FILTERED_ROBOT_HPP_ +#define FILTERED_ROBOT_HPP_ #include "kalman/ExtendedKalmanFilter.hpp" #include "filter_types.hpp" @@ -31,41 +31,41 @@ #include class FilteredRobot { - public: - FilteredRobot(const RobotTrack &track, ateam_common::TeamColor team_color); +public: + FilteredRobot(const RobotTrack & track, ateam_common::TeamColor team_color); - void update(const RobotTrack &track); + void update(const RobotTrack & track); - ateam_msgs::msg::VisionStateRobot toMsg(); + ateam_msgs::msg::VisionStateRobot toMsg(); - int getId() const; + int getId() const; - bool isHealthy() const; + bool isHealthy() const; - private: - int bot_id; - ateam_common::TeamColor team; - int age = 0; - int oldEnough = 3; - int health = 2; - int maxHealth = 20; - double maxDistance = -1.0; - std::chrono::milliseconds update_threshold{50}; - std::chrono::time_point timestamp; - std::chrono::time_point last_visible_timestamp; +private: + int bot_id; + ateam_common::TeamColor team; + int age = 0; + int oldEnough = 3; + int health = 2; + int maxHealth = 20; + double maxDistance = -1.0; + std::chrono::milliseconds update_threshold{50}; + std::chrono::time_point timestamp; + std::chrono::time_point last_visible_timestamp; // double height; // in m // Use the below filtered values when getting X/Y/w (theta) and // velocities // X, Y - Kalman::ExtendedKalmanFilter posFilterXY; - PosSystemModel systemModelXY; - PosState posXYEstimate{}; - PosMeasurementModel measurementModelXY; + Kalman::ExtendedKalmanFilter posFilterXY; + PosSystemModel systemModelXY; + PosState posXYEstimate{}; + PosMeasurementModel measurementModelXY; // Theta - Kalman::ExtendedKalmanFilter posFilterW; - AngleSystemModel systemModelW; - AngleState posWEstimate{}; - AngleMeasurementModel measurementModelW; + Kalman::ExtendedKalmanFilter posFilterW; + AngleSystemModel systemModelW; + AngleState posWEstimate{}; + AngleMeasurementModel measurementModelW; }; -#endif // FILTERED_ROBOT_HPP_ \ No newline at end of file +#endif // FILTERED_ROBOT_HPP_ diff --git a/new_super_vision_filter/src/frame_info.hpp b/new_super_vision_filter/src/frame_info.hpp deleted file mode 100644 index 7cecd0073..000000000 --- a/new_super_vision_filter/src/frame_info.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef FRAME_INFO_HPP_ -#define FRAME_INFO_HPP_ - -#include - -class LastFrameInfo { - public: - std::chrono::steady_clock::time_point time_processed; -}; - -#endif // FRAME_INFO_HPP_ diff --git a/new_super_vision_filter/src/measurements/ball_track.hpp b/new_super_vision_filter/src/measurements/ball_track.hpp index ed22302a2..94dccb91e 100644 --- a/new_super_vision_filter/src/measurements/ball_track.hpp +++ b/new_super_vision_filter/src/measurements/ball_track.hpp @@ -26,17 +26,19 @@ #include class BallTrack { - public: - BallTrack(const ssl_league_msgs::msg::VisionDetectionBall &ball_detection, int &camera_id) : camera_id(camera_id) { - PosMeasurement pos; - pos << ball_detection.pos.x, - ball_detection.pos.y; - timestamp = std::chrono::steady_clock::now(); - } +public: + BallTrack(const ssl_league_msgs::msg::VisionDetectionBall & ball_detection, int & camera_id) + : camera_id(camera_id) + { + PosMeasurement pos; + pos << ball_detection.pos.x, + ball_detection.pos.y; + timestamp = std::chrono::steady_clock::now(); + } - PosMeasurement pos; - std::chrono::time_point timestamp; - int camera_id; + PosMeasurement pos; + std::chrono::time_point timestamp; + int camera_id; }; -#endif // BALL_TRACK_HPP_ \ No newline at end of file +#endif // BALL_TRACK_HPP_ diff --git a/new_super_vision_filter/src/measurements/robot_track.hpp b/new_super_vision_filter/src/measurements/robot_track.hpp index 6c350da41..3527775e1 100644 --- a/new_super_vision_filter/src/measurements/robot_track.hpp +++ b/new_super_vision_filter/src/measurements/robot_track.hpp @@ -31,36 +31,40 @@ * used to provide updates to the Kalman filter. */ class RobotTrack { - public: - RobotTrack( - const ssl_league_msgs::msg::VisionDetectionRobot &bot_detection, - int &camera_id, - ateam_common::TeamColor &team - ) : camera_id(camera_id), team(team) { - PosMeasurement pos; - pos << bot_detection.pose.position.x, - bot_detection.pose.position.y; - angle << bot_detection.pose.orientation.w; - robot_id = bot_detection.robot_id; - } +public: + RobotTrack( + const ssl_league_msgs::msg::VisionDetectionRobot & bot_detection, + int & camera_id, + ateam_common::TeamColor & team + ) + : camera_id(camera_id), team(team) + { + PosMeasurement pos; + pos << bot_detection.pose.position.x, + bot_detection.pose.position.y; + angle << bot_detection.pose.orientation.w; + robot_id = bot_detection.robot_id; + } - int getId() const { - return robot_id; - }; + int getId() const + { + return robot_id; + } - std::chrono::time_point getTimestamp() const { - return timestamp; - } - - PosMeasurement pos; - AngleMeasurement angle; - - private: - std::chrono::time_point timestamp; - int camera_id; - ateam_common::TeamColor team; - int robot_id; + std::chrono::time_point getTimestamp() const + { + return timestamp; + } + + PosMeasurement pos; + AngleMeasurement angle; + +private: + std::chrono::time_point timestamp; + int camera_id; + ateam_common::TeamColor team; + int robot_id; }; -#endif // ROBOT_TRACK_HPP_ \ No newline at end of file +#endif // ROBOT_TRACK_HPP_ diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 82948bbd6..b5f502555 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -35,11 +35,12 @@ using namespace std::chrono_literals; -namespace new_super_vision { +namespace new_super_vision +{ class VisionFilterNode : public rclcpp::Node { - // Important things this needs to do, which could be broken out into objects that this holds + // Important things this needs to do, which could be broken out into objects that this holds // and uses when the node is ticked... // Check quality of stuff overall @@ -47,215 +48,218 @@ class VisionFilterNode : public rclcpp::Node // Fuse the info from those cameras into tracks // Output final prediction for robots and ball(s) - public: - explicit VisionFilterNode(const rclcpp::NodeOptions & options) - : rclcpp::Node("new_ateam_vision_filter", options), - game_controller_listener_(*this){ - ssl_vision_subscription_ = - create_subscription( +public: + explicit VisionFilterNode(const rclcpp::NodeOptions & options) + : rclcpp::Node("new_ateam_vision_filter", options), + game_controller_listener_(*this) + { + ssl_vision_subscription_ = + create_subscription( std::string(Topics::kVisionMessages), 10, std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1) - ); + ); + + timer_ = create_wall_timer(10ms, std::bind(&VisionFilterNode::timer_callback, this)); - timer_ = create_wall_timer(10ms, std::bind(&VisionFilterNode::timer_callback, this)); - - ball_publisher_ = create_publisher( + ball_publisher_ = create_publisher( std::string(Topics::kBall), rclcpp::SystemDefaultsQoS()); - - ateam_common::indexed_topic_helpers::create_indexed_publishers - ( + + ateam_common::indexed_topic_helpers::create_indexed_publishers + ( blue_robots_publisher_, Topics::kBlueTeamRobotPrefix, rclcpp::SystemDefaultsQoS(), this - ); - ateam_common::indexed_topic_helpers::create_indexed_publishers - ( + ); + ateam_common::indexed_topic_helpers::create_indexed_publishers + ( yellow_robots_publisher_, Topics::kYellowTeamRobotPrefix, rclcpp::SystemDefaultsQoS(), this - ); - - ssl_vision_subscription_ = - create_subscription( + ); + + ssl_vision_subscription_ = + create_subscription( std::string(Topics::kVisionMessages), 10, std::bind(&VisionFilterNode::vision_callback, this, std::placeholders::_1)); - field_subscription_ = - create_subscription( + field_subscription_ = + create_subscription( std::string(Topics::kField), 10, std::bind(&VisionFilterNode::field_callback, this, std::placeholders::_1)); - } + } - // Will also need to add publishers here and wall clock timer - - private: - std::map cameras; - rclcpp::TimerBase::SharedPtr timer_; + // Will also need to add publishers here and wall clock timer - std::vector blue_robots; - std::vector yellow_robots; - std::optional ball; +private: + std::map cameras; + rclcpp::TimerBase::SharedPtr timer_; + + std::vector blue_robots; + std::vector yellow_robots; + std::optional ball; + + std::vector ball_tracks; + std::vector blue_tracks; + std::vector yellow_tracks; - std::vector ball_tracks; - std::vector blue_tracks; - std::vector yellow_tracks; - // All this stuff interacts with other nodes (pub/sub related) - std::array::SharedPtr, - 16> blue_robots_publisher_; - std::array::SharedPtr, - 16> yellow_robots_publisher_; - rclcpp::Publisher::SharedPtr ball_publisher_; - - ateam_common::GameControllerListener game_controller_listener_; - rclcpp::Subscription::SharedPtr ssl_vision_subscription_; + std::array::SharedPtr, + 16> blue_robots_publisher_; + std::array::SharedPtr, + 16> yellow_robots_publisher_; + rclcpp::Publisher::SharedPtr ball_publisher_; + + ateam_common::GameControllerListener game_controller_listener_; + rclcpp::Subscription::SharedPtr ssl_vision_subscription_; // The two below are in case we are sharing a field during testing at event - rclcpp::Subscription::SharedPtr field_subscription_; - int ignore_side_; + rclcpp::Subscription::SharedPtr field_subscription_; + int ignore_side_; - void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) { + void vision_callback(const ssl_league_msgs::msg::VisionWrapper::SharedPtr vision_wrapper_msg) + { // Add detections to the queues - if (!vision_wrapper_msg->detection.empty()){ - for (const auto& detection : vision_wrapper_msg->detection){ - int detect_camera = detection.camera_id; + if (!vision_wrapper_msg->detection.empty()) { + for (const auto & detection : vision_wrapper_msg->detection) { + int detect_camera = detection.camera_id; // Create a new camera if we haven't seen this one before - if (!(cameras.contains(detect_camera))){ - cameras.try_emplace(detect_camera, detect_camera); - } + if (!(cameras.contains(detect_camera))) { + cameras.try_emplace(detect_camera, detect_camera); + } // Create a track from each robot in this message - for (const auto& bot : detection.robots_yellow) { - ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow; - yellow_tracks.push_back( + for (const auto & bot : detection.robots_yellow) { + ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow; + yellow_tracks.push_back( RobotTrack(bot, detect_camera, team_color) - ); - } + ); + } - for (const auto& bot : detection.robots_blue) { - ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; - blue_tracks.push_back( + for (const auto & bot : detection.robots_blue) { + ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; + blue_tracks.push_back( RobotTrack(bot, detect_camera, team_color) - ); - } + ); + } // Create a track from each ball in this message - for (const auto& ball: detection.balls) { - ball_tracks.push_back( + for (const auto & ball: detection.balls) { + ball_tracks.push_back( BallTrack(ball, detect_camera) - ); - } - } + ); + } + } // Sort our tracks to make sure they are in order of the time received // in case created/processed them out of order // Process all the new updates from the tracks - for (const auto& bot_track : blue_tracks){ - ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; - auto it = std::find_if( + for (const auto & bot_track : blue_tracks) { + ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; + auto it = std::find_if( blue_robots.begin(), blue_robots.end(), - [bot_track](const FilteredRobot &bot){ return bot_track.getId() == bot.getId(); } - ); - if (it != blue_robots.end()){ - blue_robots.push_back( + [bot_track](const FilteredRobot & bot){return bot_track.getId() == bot.getId();} + ); + if (it != blue_robots.end()) { + blue_robots.push_back( FilteredRobot(bot_track, team_color) - ); - } else { - it->update(bot_track); - } - } - for (const auto& bot_track : yellow_tracks){ - ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow; - auto it = std::find_if( + ); + } else { + it->update(bot_track); + } + } + for (const auto & bot_track : yellow_tracks) { + ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow; + auto it = std::find_if( yellow_robots.begin(), yellow_robots.end(), - [bot_track](const FilteredRobot &bot){ return bot_track.getId() == bot.getId(); } - ); - if (it != yellow_robots.end()){ - yellow_robots.push_back( + [bot_track](const FilteredRobot & bot){return bot_track.getId() == bot.getId();} + ); + if (it != yellow_robots.end()) { + yellow_robots.push_back( FilteredRobot(bot_track, team_color) - ); - } else { - it->update(bot_track); - } - } - for (const auto& ball_track : ball_tracks){ - if (!ball.has_value()){ - ball = FilteredBall(ball_track); - } else { - ball->update(ball_track); - } - } - } - - return; + ); + } else { + it->update(bot_track); + } + } + for (const auto & ball_track : ball_tracks) { + if (!ball.has_value()) { + ball = FilteredBall(ball_track); + } else { + ball->update(ball_track); } + } + } - void timer_callback() { + return; + } + + void timer_callback() + { // Remove the ball if it's bad quality - if (ball.has_value()){ - if (!ball.value().isHealthy()){ - ball.reset(); - } - } - // Erase any robots that are bad quality - std::erase_if(blue_robots, [](FilteredRobot &bot) { return !bot.isHealthy();}); - std::erase_if(yellow_robots, [](FilteredRobot &bot) { return !bot.isHealthy();}); + if (ball.has_value()) { + if (!ball.value().isHealthy()) { + ball.reset(); + } + } + // Erase any robots that are bad quality + std::erase_if(blue_robots, [](FilteredRobot & bot) {return !bot.isHealthy();}); + std::erase_if(yellow_robots, [](FilteredRobot & bot) {return !bot.isHealthy();}); // Publish the most recent ball - ateam_msgs::msg::VisionStateBall ball_msg{}; - if (ball.has_value()){ - ball_msg = ball.value().toMsg(); - } - ball_publisher_->publish(ball_msg); + ateam_msgs::msg::VisionStateBall ball_msg{}; + if (ball.has_value()) { + ball_msg = ball.value().toMsg(); + } + ball_publisher_->publish(ball_msg); // Publish the robots // - Blue - - for (std::size_t id = 0; id < 16; id++) { - auto robot_msg = ateam_msgs::msg::VisionStateRobot{}; - auto it = std::find_if( + for (std::size_t id = 0; id < 16; id++) { + auto robot_msg = ateam_msgs::msg::VisionStateRobot{}; + auto it = std::find_if( blue_robots.begin(), blue_robots.end(), - [id](const FilteredRobot &bot){ return id == bot.getId(); } - ); - if (it != blue_robots.end()){ - robot_msg = it->toMsg(); - } - blue_robots_publisher_.at(id)->publish(robot_msg); - } + [id](const FilteredRobot & bot){return id == bot.getId();} + ); + if (it != blue_robots.end()) { + robot_msg = it->toMsg(); + } + blue_robots_publisher_.at(id)->publish(robot_msg); + } // - Yellow - - for (std::size_t id = 0; id < 16; id++) { - auto robot_msg = ateam_msgs::msg::VisionStateRobot{}; - auto it = std::find_if( + for (std::size_t id = 0; id < 16; id++) { + auto robot_msg = ateam_msgs::msg::VisionStateRobot{}; + auto it = std::find_if( yellow_robots.begin(), yellow_robots.end(), - [id](const FilteredRobot &bot){ return id == bot.getId(); } - ); - if (it != yellow_robots.end()){ - robot_msg = it->toMsg(); - } - yellow_robots_publisher_.at(id)->publish(robot_msg); - } - return; - } - - void field_callback( - const ateam_msgs::msg::FieldInfo::SharedPtr field_info_msg) - { - const auto team_side = game_controller_listener_.GetTeamSide(); - if (team_side == ateam_common::TeamSide::PositiveHalf) { - ignore_side_ = -field_info_msg->ignore_side; - } else { - ignore_side_ = field_info_msg->ignore_side; - } - } + [id](const FilteredRobot & bot){return id == bot.getId();} + ); + if (it != yellow_robots.end()) { + robot_msg = it->toMsg(); + } + yellow_robots_publisher_.at(id)->publish(robot_msg); + } + return; + } + + void field_callback( + const ateam_msgs::msg::FieldInfo::SharedPtr field_info_msg) + { + const auto team_side = game_controller_listener_.GetTeamSide(); + if (team_side == ateam_common::TeamSide::PositiveHalf) { + ignore_side_ = -field_info_msg->ignore_side; + } else { + ignore_side_ = field_info_msg->ignore_side; + } + } }; } // namespace new_super_vision diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp index 4051b89a6..1cf20b479 100644 --- a/new_super_vision_filter/test/filter_test.cpp +++ b/new_super_vision_filter/test/filter_test.cpp @@ -27,24 +27,24 @@ class FilteredRobotTest : public ::testing::Test { - protected: - void SetUp() override - { - ssl_league_msgs::msg::VisionDetectionRobot robot_msg{}; - auto team = ateam_common::TeamColor::Blue; - } +protected: + void SetUp() override + { + ssl_league_msgs::msg::VisionDetectionRobot robot_msg{}; + auto team = ateam_common::TeamColor::Blue; + } }; TEST(RobotSetupTest, InitalState) { // Check that initial state is valid - return; + return; } TEST(RobotSetupTest, InitialCovariance) { // Check that initial covariance is valid - return; + return; } TEST(RobotUpdateTest, WaitUntilOldEnough) @@ -54,7 +54,7 @@ TEST(RobotUpdateTest, WaitUntilOldEnough) // Check that we didn't // Do that again until "oldEnough" // Now check that it has updated - return; + return; } TEST(RobotUpdateTest, UpdateIfTimestampValid) @@ -63,7 +63,7 @@ TEST(RobotUpdateTest, UpdateIfTimestampValid) // Create valid timestamp and fake measurement // Update the filter // Check that the current estimate is different - return; + return; } TEST(RobotUpdateTest, DontUpdateIfTimestampInvalid) @@ -72,5 +72,5 @@ TEST(RobotUpdateTest, DontUpdateIfTimestampInvalid) // Create invalid timestamp and fake measurement // Attempt to update the filter // Check that the current estimate is not different - return; -} \ No newline at end of file + return; +} From 2103c9e31a4fe3a98ef27f2f6c571cefc88c4554 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Mon, 16 Feb 2026 22:42:46 -0500 Subject: [PATCH 26/35] WIP test --- new_super_vision_filter/src/camera.hpp | 1 - new_super_vision_filter/test/filter_test.cpp | 65 +++++++++----------- 2 files changed, 29 insertions(+), 37 deletions(-) diff --git a/new_super_vision_filter/src/camera.hpp b/new_super_vision_filter/src/camera.hpp index 132c4f0f6..bc2bf3708 100644 --- a/new_super_vision_filter/src/camera.hpp +++ b/new_super_vision_filter/src/camera.hpp @@ -26,7 +26,6 @@ #include #include #include -#include "frame_info.hpp" #include "filtered_ball.hpp" #include "filtered_robot.hpp" diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp index 1cf20b479..4e4d3020b 100644 --- a/new_super_vision_filter/test/filter_test.cpp +++ b/new_super_vision_filter/test/filter_test.cpp @@ -19,11 +19,15 @@ // THE SOFTWARE. #include +#include +#include #include +#include #include #include "filtered_robot.hpp" +#include "measurements/robot_track.hpp" class FilteredRobotTest : public ::testing::Test { @@ -31,46 +35,35 @@ class FilteredRobotTest : public ::testing::Test void SetUp() override { ssl_league_msgs::msg::VisionDetectionRobot robot_msg{}; + int camera = 0; auto team = ateam_common::TeamColor::Blue; + auto track = RobotTrack(robot_msg, team); + auto bot = FilteredRobot(track, team); + int oldEnoughAge = 3; } }; -TEST(RobotSetupTest, InitalState) -{ - // Check that initial state is valid - return; -} - -TEST(RobotSetupTest, InitialCovariance) -{ - // Check that initial covariance is valid - return; -} - TEST(RobotUpdateTest, WaitUntilOldEnough) { - // Create valid timestamp and fake measurement - // Try to update - // Check that we didn't - // Do that again until "oldEnough" - // Now check that it has updated - return; -} - -TEST(RobotUpdateTest, UpdateIfTimestampValid) -{ - // Get initial estimate - // Create valid timestamp and fake measurement - // Update the filter - // Check that the current estimate is different - return; -} - -TEST(RobotUpdateTest, DontUpdateIfTimestampInvalid) -{ - // Get initial estimate - // Create invalid timestamp and fake measurement - // Attempt to update the filter - // Check that the current estimate is not different - return; + ateam_msgs::msg::VisionStateRobot default_msg{}; + for (size_t i = 0; i < (oldEnoughAge + 1); ++i){ + ssl_league_msgs::msg::VisionDetectionRobot fake_vision_data{}; + fake_vision_data.pose.position.x = 1; + fake_vision_data.pose.position.y = 1; + if (i < oldEnoughAge) { + auto fake_track = RobotTrack(fake_vision_data, team); + bot.update(fake_track); + auto msg = bot.toMsg() + // We shouldn't update if our filter is too new + EXPECT_EQ(default_msg, msg); + } else { + // Sorry, adding a short sleep was easier than mocking the timestamp... + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + auto fake_track = RobotTrack(fake_vision_data, team); + bot.update(fake_track); + auto msg = bot.toMsg() + // We should update if our filter is old enough + EXPECT_NE(default_msg, msg); + } + } } From f00936baf4b09814c858c031603cb856ccd7b236 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 17 Feb 2026 22:28:55 -0500 Subject: [PATCH 27/35] Tests compile now --- new_super_vision_filter/CMakeLists.txt | 2 ++ .../src/filtered_robot.cpp | 2 +- .../src/filtered_robot.hpp | 2 +- new_super_vision_filter/test/filter_test.cpp | 30 +++++++++++-------- 4 files changed, 22 insertions(+), 14 deletions(-) diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt index 146f22b70..652ca5c3a 100644 --- a/new_super_vision_filter/CMakeLists.txt +++ b/new_super_vision_filter/CMakeLists.txt @@ -34,6 +34,7 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp_components ateam_msgs ateam_common + ssl_league_msgs Eigen3 ) rclcpp_components_register_node( @@ -51,6 +52,7 @@ install(TARGETS ${PROJECT_NAME} if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 62af37bdb..0d0cd9dea 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -31,7 +31,7 @@ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ // OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp -FilteredRobot::FilteredRobot(const RobotTrack & track, ateam_common::TeamColor team_color) +FilteredRobot::FilteredRobot(const RobotTrack & track, ateam_common::TeamColor & team_color) : posFilterXY(), posFilterW(), bot_id(track.getId()), team(team_color) { // Initialize XY KF diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 487f72d89..189834b08 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -32,7 +32,7 @@ class FilteredRobot { public: - FilteredRobot(const RobotTrack & track, ateam_common::TeamColor team_color); + FilteredRobot(const RobotTrack & track, ateam_common::TeamColor &team_color); void update(const RobotTrack & track); diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp index 4e4d3020b..28f701f3e 100644 --- a/new_super_vision_filter/test/filter_test.cpp +++ b/new_super_vision_filter/test/filter_test.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -32,36 +33,41 @@ class FilteredRobotTest : public ::testing::Test { protected: + ateam_common::TeamColor team = ateam_common::TeamColor::Blue; + int oldEnoughAge = 3; + ssl_league_msgs::msg::VisionDetectionRobot robot_msg{}; + std::unique_ptr bot; + void SetUp() override { - ssl_league_msgs::msg::VisionDetectionRobot robot_msg{}; int camera = 0; - auto team = ateam_common::TeamColor::Blue; - auto track = RobotTrack(robot_msg, team); - auto bot = FilteredRobot(track, team); - int oldEnoughAge = 3; + auto track = RobotTrack(robot_msg, camera, team); + bot = std::make_unique(track, team); } }; -TEST(RobotUpdateTest, WaitUntilOldEnough) +TEST_F(FilteredRobotTest, WaitUntilOldEnough) { ateam_msgs::msg::VisionStateRobot default_msg{}; + int camera = 0; for (size_t i = 0; i < (oldEnoughAge + 1); ++i){ + // Set it up so we can print our messages more effectively + // https://google.github.io/googletest/advanced.html#teaching-googletest-how-to-print-your-values ssl_league_msgs::msg::VisionDetectionRobot fake_vision_data{}; fake_vision_data.pose.position.x = 1; fake_vision_data.pose.position.y = 1; if (i < oldEnoughAge) { - auto fake_track = RobotTrack(fake_vision_data, team); - bot.update(fake_track); - auto msg = bot.toMsg() + auto fake_track = RobotTrack(fake_vision_data, camera, team); + bot->update(fake_track); + auto msg = bot->toMsg(); // We shouldn't update if our filter is too new EXPECT_EQ(default_msg, msg); } else { // Sorry, adding a short sleep was easier than mocking the timestamp... std::this_thread::sleep_for(std::chrono::milliseconds(60)); - auto fake_track = RobotTrack(fake_vision_data, team); - bot.update(fake_track); - auto msg = bot.toMsg() + auto fake_track = RobotTrack(fake_vision_data, camera, team); + bot->update(fake_track); + auto msg = bot->toMsg(); // We should update if our filter is old enough EXPECT_NE(default_msg, msg); } From 0cbd70f8b234598cc710bf735335cd4871b3cf03 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 1 Mar 2026 22:28:17 -0500 Subject: [PATCH 28/35] Fix test iterator, uncrustify kalman package --- .../include/kalman/ExtendedKalmanFilter.hpp | 144 ++++---- .../include/kalman/KalmanFilterBase.hpp | 64 ++-- .../kalman/LinearizedMeasurementModel.hpp | 73 ++-- .../include/kalman/LinearizedSystemModel.hpp | 74 ++-- .../include/kalman/Matrix.hpp | 172 ++++----- .../include/kalman/MeasurementModel.hpp | 45 +-- .../include/kalman/SquareRootBase.hpp | 75 ++-- .../kalman/SquareRootExtendedKalmanFilter.hpp | 238 ++++++------ .../include/kalman/SquareRootFilterBase.hpp | 23 +- .../SquareRootUnscentedKalmanFilter.hpp | 340 +++++++++--------- .../include/kalman/StandardBase.hpp | 78 ++-- .../include/kalman/StandardFilterBase.hpp | 23 +- .../include/kalman/SystemModel.hpp | 44 +-- .../include/kalman/Types.hpp | 36 +- .../include/kalman/UnscentedKalmanFilter.hpp | 303 ++++++++-------- .../kalman/UnscentedKalmanFilterBase.hpp | 245 +++++++------ .../src/filtered_robot.hpp | 2 +- new_super_vision_filter/test/filter_test.cpp | 54 ++- 18 files changed, 1054 insertions(+), 979 deletions(-) diff --git a/new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp index 6441e9bb4..2036307f6 100644 --- a/new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp +++ b/new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp @@ -27,76 +27,77 @@ #include "LinearizedSystemModel.hpp" #include "LinearizedMeasurementModel.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Extended Kalman Filter (EKF) - * + * * This implementation is based upon [An Introduction to the Kalman Filter](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) * by Greg Welch and Gary Bishop. * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class ExtendedKalmanFilter : public KalmanFilterBase, - public StandardFilterBase - { - public: +template +class ExtendedKalmanFilter : public KalmanFilterBase, + public StandardFilterBase +{ +public: //! Kalman Filter base type - typedef KalmanFilterBase KalmanBase; + typedef KalmanFilterBase KalmanBase; //! Standard Filter base type - typedef StandardFilterBase StandardBase; - + typedef StandardFilterBase StandardBase; + //! Numeric Scalar Type inherited from base - using typename KalmanBase::T; - + using typename KalmanBase::T; + //! State Type inherited from base - using typename KalmanBase::State; - + using typename KalmanBase::State; + //! Linearized Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = LinearizedMeasurementModel; - + template class CovarianceBase> + using MeasurementModelType = LinearizedMeasurementModel; + //! Linearized System Model Type - template class CovarianceBase> - using SystemModelType = LinearizedSystemModel; - - protected: + template class CovarianceBase> + using SystemModelType = LinearizedSystemModel; + +protected: //! Kalman Gain Matrix Type - template - using KalmanGain = Kalman::KalmanGain; - - protected: + template + using KalmanGain = Kalman::KalmanGain; + +protected: //! State Estimate - using KalmanBase::x; + using KalmanBase::x; //! State Covariance Matrix - using StandardBase::P; - - public: + using StandardBase::P; + +public: /** * @brief Constructor */ - ExtendedKalmanFilter() - { + ExtendedKalmanFilter() + { // Setup state and covariance - P.setIdentity(); - } - + P.setIdentity(); + } + /** * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) * * @param [in] s The System model * @return The updated state estimate */ - template class CovarianceBase> - const State& predict( SystemModelType& s ) - { + template class CovarianceBase> + const State & predict(SystemModelType & s) + { // predict state (without control) - Control u; - u.setZero(); - return predict( s, u ); - } - + Control u; + u.setZero(); + return predict(s, u); + } + /** * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model * @@ -104,21 +105,21 @@ namespace Kalman { * @param [in] u The Control input vector * @return The updated state estimate */ - template class CovarianceBase> - const State& predict( SystemModelType& s, const Control& u ) - { - s.updateJacobians( x, u ); - + template class CovarianceBase> + const State & predict(SystemModelType & s, const Control & u) + { + s.updateJacobians(x, u); + // predict state - x = s.f(x, u); - + x = s.f(x, u); + // predict covariance - P = ( s.F * P * s.F.transpose() ) + ( s.W * s.getCovariance() * s.W.transpose() ); - + P = ( s.F * P * s.F.transpose() ) + ( s.W * s.getCovariance() * s.W.transpose() ); + // return state prediction - return this->getState(); - } - + return this->getState(); + } + /** * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model * @@ -126,29 +127,30 @@ namespace Kalman { * @param [in] z The measurement vector * @return The updated state estimate */ - template class CovarianceBase> - const State& update( MeasurementModelType& m, const Measurement& z ) - { - m.updateJacobians( x ); - + template class CovarianceBase> + const State & update(MeasurementModelType & m, const Measurement & z) + { + m.updateJacobians(x); + // COMPUTE KALMAN GAIN // compute innovation covariance - Covariance S = ( m.H * P * m.H.transpose() ) + ( m.V * m.getCovariance() * m.V.transpose() ); - + Covariance S = ( m.H * P * m.H.transpose() ) + + ( m.V * m.getCovariance() * m.V.transpose() ); + // compute kalman gain - KalmanGain K = P * m.H.transpose() * S.inverse(); - + KalmanGain K = P * m.H.transpose() * S.inverse(); + // UPDATE STATE ESTIMATE AND COVARIANCE // Update state using computed kalman gain and innovation - x += K * ( z - m.h( x ) ); - + x += K * ( z - m.h(x) ); + // Update covariance - P -= K * m.H * P; - + P -= K * m.H * P; + // return updated state estimate - return this->getState(); - } - }; + return this->getState(); + } +}; } #endif diff --git a/new_super_vision_filter/include/kalman/KalmanFilterBase.hpp b/new_super_vision_filter/include/kalman/KalmanFilterBase.hpp index ffe5faeab..b24dfa0e8 100644 --- a/new_super_vision_filter/include/kalman/KalmanFilterBase.hpp +++ b/new_super_vision_filter/include/kalman/KalmanFilterBase.hpp @@ -25,56 +25,58 @@ #include "Matrix.hpp" #include "Types.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Abstract base class for all Kalman Filters - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class KalmanFilterBase - { - public: - static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/StateType::RowsAtCompileTime > 0, +template +class KalmanFilterBase +{ +public: + static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/StateType::RowsAtCompileTime > 0, "State vector must contain at least 1 element" /* or be dynamic */); - static_assert(StateType::ColsAtCompileTime == 1, "State type must be a column vector"); + static_assert(StateType::ColsAtCompileTime == 1, "State type must be a column vector"); //! Numeric scalar type - typedef typename StateType::Scalar T; - + typedef typename StateType::Scalar T; + //! Type of the state vector - typedef StateType State; - - protected: + typedef StateType State; + +protected: //! Estimated state - State x; - - public: + State x; + +public: /** * Get current state estimate */ - const State& getState() const - { - return x; - } - + const State & getState() const + { + return x; + } + /** * @brief Initialize state * @param initialState The initial state of the system */ - void init(const State& initialState) - { - x = initialState; - } - protected: + void init(const State & initialState) + { + x = initialState; + } + +protected: /** * @brief Protected constructor */ - KalmanFilterBase() - { - } - }; + KalmanFilterBase() + { + } +}; } #endif diff --git a/new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp b/new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp index e3831f1d4..e3d4c3ebf 100644 --- a/new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp +++ b/new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp @@ -24,12 +24,13 @@ #include "MeasurementModel.hpp" -namespace Kalman { - template - class ExtendedKalmanFilter; - template - class SquareRootExtendedKalmanFilter; - +namespace Kalman +{ +template +class ExtendedKalmanFilter; +template +class SquareRootExtendedKalmanFilter; + /** * @brief Abstract base class of all linearized (first order taylor expansion) measurement models * @@ -37,43 +38,47 @@ namespace Kalman { * @param MeasurementType The vector-type of the measurement (usually some type derived from Kalman::Vector) * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) */ - template class CovarianceBase = StandardBase> - class LinearizedMeasurementModel : public MeasurementModel - { - friend class ExtendedKalmanFilter; - friend class SquareRootExtendedKalmanFilter; - public: +template class CovarianceBase = StandardBase> +class LinearizedMeasurementModel : public MeasurementModel +{ + friend class ExtendedKalmanFilter; + friend class SquareRootExtendedKalmanFilter; + +public: //! Measurement model base - typedef MeasurementModel Base; - + typedef MeasurementModel Base; + //! System state type - using typename Base::State; - + using typename Base::State; + //! Measurement vector type - using typename Base::Measurement; - - protected: + using typename Base::Measurement; + +protected: //! Measurement model jacobian - Jacobian H; + Jacobian H; //! Measurement model noise jacobian - Jacobian V; - + Jacobian V; + /** * Callback function for state-dependent update of Jacobi-matrices H and V before each update step */ - virtual void updateJacobians( const State& x ) - { + virtual void updateJacobians(const State & x) + { // No update by default - (void)x; - } - protected: - LinearizedMeasurementModel() - { - H.setIdentity(); - V.setIdentity(); - } - ~LinearizedMeasurementModel() {} - }; + (void)x; + } + +protected: + LinearizedMeasurementModel() + { + H.setIdentity(); + V.setIdentity(); + } + ~LinearizedMeasurementModel() {} +}; } #endif diff --git a/new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp b/new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp index 836cc91cb..1cc197058 100644 --- a/new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp +++ b/new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp @@ -24,12 +24,13 @@ #include "SystemModel.hpp" -namespace Kalman { - template - class ExtendedKalmanFilter; - template - class SquareRootExtendedKalmanFilter; - +namespace Kalman +{ +template +class ExtendedKalmanFilter; +template +class SquareRootExtendedKalmanFilter; + /** * @brief Abstract base class of all linearized (first order taylor expansion) system models * @@ -37,44 +38,47 @@ namespace Kalman { * @param ControlType The vector-type of the control input (usually some type derived from Kalman::Vector) * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) */ - template, template class CovarianceBase = StandardBase > - class LinearizedSystemModel : public SystemModel - { - friend class ExtendedKalmanFilter; - friend class SquareRootExtendedKalmanFilter; - public: +template, + template class CovarianceBase = StandardBase> +class LinearizedSystemModel : public SystemModel +{ + friend class ExtendedKalmanFilter; + friend class SquareRootExtendedKalmanFilter; + +public: //! System model base - typedef SystemModel Base; - + typedef SystemModel Base; + //! System state type - using typename Base::State; - + using typename Base::State; + //! System control input type - using typename Base::Control; - - protected: + using typename Base::Control; + +protected: //! System model jacobian - Jacobian F; + Jacobian F; //! System model noise jacobian - Jacobian W; - + Jacobian W; + /** * Callback function for state-dependent update of Jacobi-matrices F and W before each update step */ - virtual void updateJacobians( const State& x, const Control& u ) - { + virtual void updateJacobians(const State & x, const Control & u) + { // No update by default - (void)x; - (void)u; - } - protected: - LinearizedSystemModel() - { - F.setIdentity(); - W.setIdentity(); - } - ~LinearizedSystemModel() {} - }; + (void)x; + (void)u; + } + +protected: + LinearizedSystemModel() + { + F.setIdentity(); + W.setIdentity(); + } + ~LinearizedSystemModel() {} +}; } #endif diff --git a/new_super_vision_filter/include/kalman/Matrix.hpp b/new_super_vision_filter/include/kalman/Matrix.hpp index 574ebdf58..16c03ee46 100644 --- a/new_super_vision_filter/include/kalman/Matrix.hpp +++ b/new_super_vision_filter/include/kalman/Matrix.hpp @@ -26,27 +26,28 @@ #include -#define KALMAN_VECTOR(NAME, T, N) \ - typedef Kalman::Vector Base; \ - using typename Base::Scalar; \ - using Base::RowsAtCompileTime; \ - using Base::ColsAtCompileTime; \ - using Base::SizeAtCompileTime; \ - \ - NAME(void) : Kalman::Vector() {} \ - \ - template \ - NAME(const Eigen::MatrixBase& other) : Kalman::Vector(other) {} \ - \ - template \ - NAME& operator= (const Eigen::MatrixBase & other) \ - { \ - this->Base::operator=(other); \ - return *this; \ - } - -namespace Kalman { - const int Dynamic = Eigen::Dynamic; +#define KALMAN_VECTOR(NAME, T, N) \ + typedef Kalman::Vector Base; \ + using typename Base::Scalar; \ + using Base::RowsAtCompileTime; \ + using Base::ColsAtCompileTime; \ + using Base::SizeAtCompileTime; \ + \ + NAME(void) : Kalman::Vector() {} \ + \ + template \ + NAME(const Eigen::MatrixBase & other) : Kalman::Vector(other) {} \ + \ + template \ + NAME & operator=(const Eigen::MatrixBase & other) \ + { \ + this->Base::operator=(other); \ + return *this; \ + } + +namespace Kalman +{ +const int Dynamic = Eigen::Dynamic; /** * @class Kalman::Matrix @@ -55,106 +56,109 @@ namespace Kalman { * @param rows The number of rows * @param cols The number of columns */ - template - using Matrix = Eigen::Matrix; - +template +using Matrix = Eigen::Matrix; + /** * @brief Template type for vectors * @param T The numeric scalar type * @param N The vector dimension */ - template - class Vector : public Matrix - { - public: +template +class Vector : public Matrix +{ +public: //! Matrix base type - typedef Matrix Base; + typedef Matrix Base; + + using typename Base::Scalar; + using Base::RowsAtCompileTime; + using Base::ColsAtCompileTime; + using Base::SizeAtCompileTime; - using typename Base::Scalar; - using Base::RowsAtCompileTime; - using Base::ColsAtCompileTime; - using Base::SizeAtCompileTime; + Vector(void) + : Matrix() {} - Vector(void) : Matrix() {} - /** * @brief Copy constructor */ - template - Vector(const Eigen::MatrixBase& other) - : Matrix(other) - { } + template + Vector(const Eigen::MatrixBase & other) + : Matrix(other) + {} /** * @brief Copy assignment constructor */ - template - Vector& operator= (const Eigen::MatrixBase & other) - { - this->Base::operator=(other); - return *this; - } - }; - + template + Vector & operator=(const Eigen::MatrixBase & other) + { + this->Base::operator=(other); + return *this; + } +}; + /** * @brief Cholesky square root decomposition of a symmetric positive-definite matrix * @param _MatrixType The matrix type * @param _UpLo Square root form (Eigen::Lower or Eigen::Upper) */ - template - class Cholesky : public Eigen::LLT< _MatrixType, _UpLo > - { - public: - Cholesky() : Eigen::LLT< _MatrixType, _UpLo >() {} - +template +class Cholesky : public Eigen::LLT<_MatrixType, _UpLo> +{ +public: + Cholesky() + : Eigen::LLT<_MatrixType, _UpLo>() {} + /** * @brief Construct cholesky square root decomposition from matrix * @param m The matrix to be decomposed */ - Cholesky(const _MatrixType& m ) : Eigen::LLT< _MatrixType, _UpLo >(m) {} - + Cholesky(const _MatrixType & m) + : Eigen::LLT<_MatrixType, _UpLo>(m) {} + /** * @brief Set decomposition to identity */ - Cholesky& setIdentity() - { - this->m_matrix.setIdentity(); - this->m_isInitialized = true; - return *this; - } - + Cholesky & setIdentity() + { + this->m_matrix.setIdentity(); + this->m_isInitialized = true; + return *this; + } + /** * @brief Check whether the decomposed matrix is the identity matrix */ - bool isIdentity() const - { - eigen_assert(this->m_isInitialized && "LLT is not initialized."); - return this->m_matrix.isIdentity(); - } - + bool isIdentity() const + { + eigen_assert(this->m_isInitialized && "LLT is not initialized."); + return this->m_matrix.isIdentity(); + } + /** * @brief Set lower triangular part of the decomposition * @param matrix The lower part stored in a full matrix */ - template - Cholesky& setL(const Eigen::MatrixBase & matrix) - { - this->m_matrix = matrix.template triangularView(); - this->m_isInitialized = true; - return *this; - } - + template + Cholesky & setL(const Eigen::MatrixBase & matrix) + { + this->m_matrix = matrix.template triangularView(); + this->m_isInitialized = true; + return *this; + } + /** * @brief Set upper triangular part of the decomposition * @param matrix The upper part stored in a full matrix */ - template - Cholesky& setU(const Eigen::MatrixBase & matrix) - { - this->m_matrix = matrix.template triangularView().adjoint(); - this->m_isInitialized = true; - return *this; - } - }; + template + Cholesky & setU(const Eigen::MatrixBase & matrix) + { + this->m_matrix = matrix.template triangularView().adjoint(); + this->m_isInitialized = true; + return *this; + } +}; } #endif diff --git a/new_super_vision_filter/include/kalman/MeasurementModel.hpp b/new_super_vision_filter/include/kalman/MeasurementModel.hpp index b3f3b3754..f36f1a1c2 100644 --- a/new_super_vision_filter/include/kalman/MeasurementModel.hpp +++ b/new_super_vision_filter/include/kalman/MeasurementModel.hpp @@ -26,7 +26,8 @@ #include "StandardBase.hpp" -namespace Kalman { +namespace Kalman +{ /** * @brief Abstract base class of all measurement models * @@ -34,34 +35,38 @@ namespace Kalman { * @param MeasurementType The vector-type of the measurement (usually some type derived from Kalman::Vector) * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) */ - template class CovarianceBase = StandardBase> - class MeasurementModel : public CovarianceBase - { - static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/StateType::RowsAtCompileTime > 0, +template class CovarianceBase = StandardBase> +class MeasurementModel : public CovarianceBase +{ + static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/StateType::RowsAtCompileTime > 0, "State vector must contain at least 1 element" /* or be dynamic */); - static_assert(/*MeasurementType::RowsAtCompileTime == Dynamic ||*/MeasurementType::RowsAtCompileTime > 0, + static_assert( + /*MeasurementType::RowsAtCompileTime == Dynamic ||*/MeasurementType:: + RowsAtCompileTime > 0, "Measurement vector must contain at least 1 element" /* or be dynamic */); - static_assert(std::is_same::value, + static_assert(std::is_same::value, "State and Measurement scalar types must be identical"); - public: + +public: //! System state type - typedef StateType State; - + typedef StateType State; + //! Measurement vector type - typedef MeasurementType Measurement; - - public: + typedef MeasurementType Measurement; + +public: /** * Measurement Model Function h - * + * * Predicts the estimated measurement value given the current state estimate x */ - virtual Measurement h(const State& x) const = 0; - - protected: - MeasurementModel() {} - virtual ~MeasurementModel() {} - }; + virtual Measurement h(const State & x) const = 0; + +protected: + MeasurementModel() {} + virtual ~MeasurementModel() {} +}; } #endif diff --git a/new_super_vision_filter/include/kalman/SquareRootBase.hpp b/new_super_vision_filter/include/kalman/SquareRootBase.hpp index a2312b312..40ecd6a94 100644 --- a/new_super_vision_filter/include/kalman/SquareRootBase.hpp +++ b/new_super_vision_filter/include/kalman/SquareRootBase.hpp @@ -24,45 +24,46 @@ #include "Types.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Abstract base class for square-root filters and models - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class SquareRootBase - { - protected: +template +class SquareRootBase +{ +protected: //! Covariance Square Root - CovarianceSquareRoot S; - - public: + CovarianceSquareRoot S; + +public: /** * Get covariance (as square root) */ - const CovarianceSquareRoot& getCovarianceSquareRoot() const - { - return S; - } - + const CovarianceSquareRoot & getCovarianceSquareRoot() const + { + return S; + } + /** * Get covariance reconstructed from square root */ - Covariance getCovariance() const - { - return S.reconstructedMatrix(); - } - + Covariance getCovariance() const + { + return S.reconstructedMatrix(); + } + /** * Set Covariance */ - bool setCovariance(const Covariance& covariance) - { - S.compute(covariance); - return (S.info() == Eigen::Success); - } + bool setCovariance(const Covariance & covariance) + { + S.compute(covariance); + return S.info() == Eigen::Success; + } /** * @brief Set Covariance using Square Root @@ -70,18 +71,18 @@ namespace Kalman { * @param covSquareRoot Lower triangular Matrix representing the covariance * square root (i.e. P = LLˆT) */ - bool setCovarianceSquareRoot(const Covariance& covSquareRoot) - { - S.setL(covSquareRoot); - return true; - } - - protected: - SquareRootBase() - { - S.setIdentity(); - } - }; + bool setCovarianceSquareRoot(const Covariance & covSquareRoot) + { + S.setL(covSquareRoot); + return true; + } + +protected: + SquareRootBase() + { + S.setIdentity(); + } +}; } #endif diff --git a/new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp index 2f876af57..8f0a10d95 100644 --- a/new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp +++ b/new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp @@ -27,76 +27,77 @@ #include "LinearizedSystemModel.hpp" #include "LinearizedMeasurementModel.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Square-Root Extended Kalman Filter (SR-EKF) - * + * * This implementation is based upon [An Introduction to the Kalman Filter](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) * by Greg Welch and Gary Bishop. * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class SquareRootExtendedKalmanFilter : public KalmanFilterBase, - public SquareRootFilterBase - { - public: +template +class SquareRootExtendedKalmanFilter : public KalmanFilterBase, + public SquareRootFilterBase +{ +public: //! Kalman Filter base type - typedef KalmanFilterBase KalmanBase; + typedef KalmanFilterBase KalmanBase; //! SquareRoot Filter base type - typedef SquareRootFilterBase SquareRootBase; - + typedef SquareRootFilterBase SquareRootBase; + //! Numeric Scalar Type inherited from base - using typename KalmanBase::T; - + using typename KalmanBase::T; + //! State Type inherited from base - using typename KalmanBase::State; - + using typename KalmanBase::State; + //! Linearized Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = LinearizedMeasurementModel; - + template class CovarianceBase> + using MeasurementModelType = LinearizedMeasurementModel; + //! Linearized System Model Type - template class CovarianceBase> - using SystemModelType = LinearizedSystemModel; - - protected: + template class CovarianceBase> + using SystemModelType = LinearizedSystemModel; + +protected: //! Kalman Gain Matrix Type - template - using KalmanGain = Kalman::KalmanGain; - - protected: + template + using KalmanGain = Kalman::KalmanGain; + +protected: //! State estimate - using KalmanBase::x; + using KalmanBase::x; //! State covariance square root - using SquareRootBase::S; - - public: + using SquareRootBase::S; + +public: /** * @brief Constructor */ - SquareRootExtendedKalmanFilter() - { + SquareRootExtendedKalmanFilter() + { // Setup covariance - S.setIdentity(); - } - + S.setIdentity(); + } + /** * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) * * @param [in] s The System model * @return The updated state estimate */ - template class CovarianceBase> - const State& predict( SystemModelType& s ) - { + template class CovarianceBase> + const State & predict(SystemModelType & s) + { // predict state (without control) - Control u; - u.setZero(); - return predict( s, u ); - } - + Control u; + u.setZero(); + return predict(s, u); + } + /** * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model * @@ -104,21 +105,21 @@ namespace Kalman { * @param [in] u The Control input vector * @return The updated state estimate */ - template class CovarianceBase> - const State& predict( SystemModelType& s, const Control& u ) - { - s.updateJacobians( x, u ); - + template class CovarianceBase> + const State & predict(SystemModelType & s, const Control & u) + { + s.updateJacobians(x, u); + // predict state - x = s.f(x, u); - + x = s.f(x, u); + // predict covariance - computePredictedCovarianceSquareRoot(s.F, S, s.W, s.getCovarianceSquareRoot(), S); - + computePredictedCovarianceSquareRoot(s.F, S, s.W, s.getCovarianceSquareRoot(), S); + // return state prediction - return this->getState(); - } - + return this->getState(); + } + /** * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model * @@ -126,32 +127,33 @@ namespace Kalman { * @param [in] z The measurement vector * @return The updated state estimate */ - template class CovarianceBase> - const State& update( MeasurementModelType& m, const Measurement& z ) - { - m.updateJacobians( x ); - + template class CovarianceBase> + const State & update(MeasurementModelType & m, const Measurement & z) + { + m.updateJacobians(x); + // COMPUTE KALMAN GAIN // compute innovation covariance - CovarianceSquareRoot S_y; - computePredictedCovarianceSquareRoot(m.H, S, m.V, m.getCovarianceSquareRoot(), S_y); - + CovarianceSquareRoot S_y; + computePredictedCovarianceSquareRoot(m.H, S, m.V, m.getCovarianceSquareRoot(), + S_y); + // compute kalman gain - KalmanGain K; - computeKalmanGain(m.H, S_y, K); - + KalmanGain K; + computeKalmanGain(m.H, S_y, K); + // UPDATE STATE ESTIMATE AND COVARIANCE // Update state using computed kalman gain and innovation - x += K * ( z - m.h( x ) ); - + x += K * ( z - m.h(x) ); + // Update covariance - updateStateCovariance(K, m.H); - + updateStateCovariance(K, m.H); + // return updated state estimate - return this->getState(); - } - protected: - + return this->getState(); + } + +protected: /** * @brief Compute the predicted state or innovation covariance (as square root) * @@ -159,11 +161,11 @@ namespace Kalman { * deduced in a very straight forward way using the method outlined in the * [Unscented Kalman Filter Tutorial](https://cse.sc.edu/~terejanu/files/tutorialUKF.pdf) * by Gabriel A. Terejanu for the UKF (Section 3, formulas (27) and (28)). - * + * * Starting from the standard update formula - * + * * \f[ \hat{P} = FPF^T + WQW^T \f] - * + * * and using the square-root decomposition \f$ P = SS^T \f$ with \f$S\f$ being lower-triagonal * as well as the (lower triangular) square root \f$\sqrt{Q}\f$ of \f$Q\f$ this can be formulated as * @@ -181,7 +183,7 @@ namespace Kalman { * can then be decomposed into a product of matrices \f$OR\f$ with \f$O\f$ being orthogonal * and \f$R\f$ being upper triangular (also known as QR decompositon). Using this \f$\hat{P}\f$ * can be written as - * + * * \f{align*}{ * \hat{P} &= \begin{bmatrix} FS & W\sqrt{Q} \end{bmatrix} * \begin{bmatrix} (FS)^T \\ (W\sqrt{Q})^T \end{bmatrix} \\ @@ -194,10 +196,10 @@ namespace Kalman { * of the upper-triangular matrix obtained from QR-decompositon of the augmented block matrix * * \f[ \begin{bmatrix} FS & W\sqrt{Q} \end{bmatrix}^T = \begin{bmatrix} S^T F^T \\ \sqrt{Q}^T W^T \end{bmatrix} \in \mathbb{R}^{2n \times n} \f] - * + * * The same can be applied for the innovation covariance by replacing the jacobians and * the noise covariance accordingly. - * + * * @param [in] A The jacobian of state transition or measurement function w.r.t. state or measurement * @param [in] S The state covariance (as square root) * @param [in] B The jacobian of state transition or measurement function w.r.t. state or measurement @@ -205,32 +207,34 @@ namespace Kalman { * @param [out] S_pred The predicted covariance (as square root) * @return True on success, false on failure due to numerical issue */ - template - bool computePredictedCovarianceSquareRoot( const Jacobian& A, const CovarianceSquareRoot& S, - const Jacobian& B, const CovarianceSquareRoot& R, - CovarianceSquareRoot& S_pred) - { + template + bool computePredictedCovarianceSquareRoot( + const Jacobian & A, const CovarianceSquareRoot & S, + const Jacobian & B, const CovarianceSquareRoot & R, + CovarianceSquareRoot & S_pred) + { // Compute QR decomposition of (transposed) augmented matrix - Matrix tmp; - tmp.template topRows() = S.matrixU() * A.transpose(); - tmp.template bottomRows() = R.matrixU() * B.transpose(); - + Matrix tmp; + tmp.template topRows() = S.matrixU() * A.transpose(); + tmp.template bottomRows() = R.matrixU() * B.transpose(); + // TODO: Use ColPivHouseholderQR - Eigen::HouseholderQR qr( tmp ); - + Eigen::HouseholderQR qr(tmp); + // Set S_pred matrix as upper triangular square root - S_pred.setU(qr.matrixQR().template topRightCorner()); - return true; - } - + S_pred.setU(qr.matrixQR().template topRightCorner()); + return true; + } + /** * @brief Compute Kalman gain from state covariance, innovation covariance and measurement jacobian - * + * * The formula for the kalman gain using square-root covariances can be deduced from the * original update formula * * \f[ K = P H^T P_{yy}^{-1} \Leftrightarrow K P_{yy} = P H^T \f] - * + * * with \f$P_yy\f$ being the innovation covariance. With \f$P = SS^T\f$ and \f$P_yy = S_yS_y^T\f$ * and transposition of the whole equation the above can be formulated as * @@ -240,7 +244,7 @@ namespace Kalman { * (S_yS_y^T)^TK^T &= H(SS^T)^T \\ * \underbrace{S_yS_y^T}_{=P_{yy}} K^T &= HSS^T * \f} - * + * * Since \f$S_y\f$ is lower triangular, the above can be solved using backsubstitution * in an efficient manner. * @@ -248,33 +252,35 @@ namespace Kalman { * @param [in] S_y The Innovation covariance square root * @param [out] K The computed Kalman Gain */ - template - bool computeKalmanGain( const Jacobian& H, - const CovarianceSquareRoot& S_y, - KalmanGain& K) - { + template + bool computeKalmanGain( + const Jacobian & H, + const CovarianceSquareRoot & S_y, + KalmanGain & K) + { // Solve using backsubstitution // AX=B with B = HSS^T and X = K^T and A = S_yS_y^T - K = S_y.solve(H*S.reconstructedMatrix()).transpose(); - return true; - } - + K = S_y.solve(H * S.reconstructedMatrix()).transpose(); + return true; + } + /** * @brief Update state covariance using Kalman gain * * @param [in] K The Kalman gain * @param [in] H The jacobian of the measurement function w.r.t. the state */ - template - bool updateStateCovariance( const KalmanGain& K, - const Jacobian& H) - { + template + bool updateStateCovariance( + const KalmanGain & K, + const Jacobian & H) + { // TODO: update covariance without using decomposition - Matrix P = S.reconstructedMatrix(); - S.compute( (P - K * H * P).eval() ); - return (S.info() == Eigen::Success); - } - }; + Matrix P = S.reconstructedMatrix(); + S.compute( (P - K * H * P).eval() ); + return S.info() == Eigen::Success; + } +}; } #endif diff --git a/new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp b/new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp index 12d5a4271..b53641889 100644 --- a/new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp +++ b/new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp @@ -24,23 +24,24 @@ #include "SquareRootBase.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Abstract base class for square root filters - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class SquareRootFilterBase : public SquareRootBase - { - protected: +template +class SquareRootFilterBase : public SquareRootBase +{ +protected: //! SquareRoot Base Type - typedef SquareRootBase Base; - + typedef SquareRootBase Base; + //! Covariance Square Root - using Base::S; - }; + using Base::S; +}; } #endif diff --git a/new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp index f23626732..c032b9805 100644 --- a/new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp +++ b/new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp @@ -25,99 +25,101 @@ #include "UnscentedKalmanFilterBase.hpp" #include "SquareRootFilterBase.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Square Root Unscented Kalman Filter (SR-UKF) * * @note This is the square-root implementation variant of UnscentedKalmanFilter - * + * * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. * Whenever "the paper" is referenced within this file then this paper is meant. - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class SquareRootUnscentedKalmanFilter : public UnscentedKalmanFilterBase, - public SquareRootFilterBase - { - public: +template +class SquareRootUnscentedKalmanFilter : public UnscentedKalmanFilterBase, + public SquareRootFilterBase +{ +public: //! Unscented Kalman Filter base type - typedef UnscentedKalmanFilterBase UnscentedBase; - + typedef UnscentedKalmanFilterBase UnscentedBase; + //! Square Root Filter base type - typedef SquareRootFilterBase SquareRootBase; - + typedef SquareRootFilterBase SquareRootBase; + //! Numeric Scalar Type inherited from base - using typename UnscentedBase::T; - + using typename UnscentedBase::T; + //! State Type inherited from base - using typename UnscentedBase::State; - + using typename UnscentedBase::State; + //! Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = typename UnscentedBase::template MeasurementModelType; + template class CovarianceBase> + using MeasurementModelType = typename UnscentedBase::template MeasurementModelType; //! System Model Type - template class CovarianceBase> - using SystemModelType = typename UnscentedBase::template SystemModelType; - - protected: + template class CovarianceBase> + using SystemModelType = typename UnscentedBase::template SystemModelType; + +protected: //! The number of sigma points (depending on state dimensionality) - using UnscentedBase::SigmaPointCount; - + using UnscentedBase::SigmaPointCount; + //! Matrix type containing the sigma state or measurement points - template - using SigmaPoints = typename UnscentedBase::template SigmaPoints; - + template + using SigmaPoints = typename UnscentedBase::template SigmaPoints; + //! Kalman Gain Matrix Type - template - using KalmanGain = Kalman::KalmanGain; - - protected: + template + using KalmanGain = Kalman::KalmanGain; + +protected: // Member variables - + //! State Estimate - using UnscentedBase::x; - + using UnscentedBase::x; + //! Square Root of State Covariance - using SquareRootBase::S; - + using SquareRootBase::S; + //! Sigma points (state) - using UnscentedBase::sigmaStatePoints; - - public: + using UnscentedBase::sigmaStatePoints; + +public: /** * Constructor - * + * * See paper for detailed parameter explanation - * + * * @param alpha Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) * @param beta Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) * @param kappa Secondary scaling parameter (usually 0) */ - SquareRootUnscentedKalmanFilter(T alpha = T(1), T beta = T(2), T kappa = T(0)) - : UnscentedKalmanFilterBase(alpha, beta, kappa) - { + SquareRootUnscentedKalmanFilter(T alpha = T(1), T beta = T(2), T kappa = T(0)) + : UnscentedKalmanFilterBase(alpha, beta, kappa) + { // Init covariance to identity - S.setIdentity(); - } - + S.setIdentity(); + } + /** * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) * * @param [in] s The System model * @return The updated state estimate */ - template class CovarianceBase> - const State& predict( const SystemModelType& s ) - { + template class CovarianceBase> + const State & predict(const SystemModelType & s) + { // predict state (without control) - Control u; - u.setZero(); - return predict( s, u ); - } - + Control u; + u.setZero(); + return predict(s, u); + } + /** * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model * @@ -125,26 +127,27 @@ namespace Kalman { * @param [in] u The Control input vector * @return The updated state estimate */ - template class CovarianceBase> - const State& predict( const SystemModelType& s, const Control& u ) - { + template class CovarianceBase> + const State & predict(const SystemModelType & s, const Control & u) + { // Compute sigma points - computeSigmaPoints(); - + computeSigmaPoints(); + // Compute predicted state - x = this->template computeStatePrediction(s, u); - + x = this->template computeStatePrediction(s, u); + // Compute predicted covariance - if(!computeCovarianceSquareRootFromSigmaPoints(x, sigmaStatePoints, s.getCovarianceSquareRoot(), S)) - { + if(!computeCovarianceSquareRootFromSigmaPoints(x, sigmaStatePoints, s.getCovarianceSquareRoot(), + S)) + { // TODO: handle numerical error - assert(false); - } - + assert(false); + } + // Return predicted state - return this->getState(); - } - + return this->getState(); + } + /** * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model * @@ -152,66 +155,69 @@ namespace Kalman { * @param [in] z The measurement vector * @return The updated state estimate */ - template class CovarianceBase> - const State& update( const MeasurementModelType& m, const Measurement& z ) - { - SigmaPoints sigmaMeasurementPoints; - + template class CovarianceBase> + const State & update( + const MeasurementModelType & m, + const Measurement & z) + { + SigmaPoints sigmaMeasurementPoints; + // Predict measurement (and corresponding sigma points) - Measurement y = this->template computeMeasurementPrediction(m, sigmaMeasurementPoints); - + Measurement y = this->template computeMeasurementPrediction(m, + sigmaMeasurementPoints); + // Compute square root innovation covariance - CovarianceSquareRoot S_y; - if(!computeCovarianceSquareRootFromSigmaPoints(y, sigmaMeasurementPoints, m.getCovarianceSquareRoot(), S_y)) - { + CovarianceSquareRoot S_y; + if(!computeCovarianceSquareRootFromSigmaPoints(y, sigmaMeasurementPoints, + m.getCovarianceSquareRoot(), S_y)) + { // TODO: handle numerical error - assert(false); - } - - KalmanGain K; - computeKalmanGain(y, sigmaMeasurementPoints, S_y, K); - + assert(false); + } + + KalmanGain K; + computeKalmanGain(y, sigmaMeasurementPoints, S_y, K); + // Update state - x += K * ( z - y ); - + x += K * ( z - y ); + // Update state covariance - if(!updateStateCovariance(K, S_y)) - { + if(!updateStateCovariance(K, S_y)) { // TODO: handle numerical error - assert(false); - } - - return this->getState(); - } - - protected: + assert(false); + } + + return this->getState(); + } + +protected: /** * @brief Compute sigma points from current state estimate and state covariance - * + * * @note This covers equations (17) and (22) of Algorithm 3.1 in the Paper */ - bool computeSigmaPoints() - { + bool computeSigmaPoints() + { // Get square root of covariance - Matrix _S = S.matrixL().toDenseMatrix(); - + Matrix _S = S.matrixL().toDenseMatrix(); + // Set left "block" (first column) - sigmaStatePoints.template leftCols<1>() = x; + sigmaStatePoints.template leftCols<1>() = x; // Set center block with x + gamma * S - sigmaStatePoints.template block(0,1) - = ( this->gamma * _S).colwise() + x; + sigmaStatePoints.template block(0, 1) = + ( this->gamma * _S).colwise() + x; // Set right block with x - gamma * S - sigmaStatePoints.template rightCols() - = (-this->gamma * _S).colwise() + x; - - return true; - } - + sigmaStatePoints.template rightCols() = + (-this->gamma * _S).colwise() + x; + + return true; + } + /** * @brief Compute the Covariance Square root from sigma points and noise covariance - * + * * @note This covers equations (20) and (21) as well as (25) and (26) of Algorithm 3.1 in the Paper - * + * * @param [in] mean The mean predicted state or measurement * @param [in] sigmaPoints the predicted sigma state or measurement points * @param [in] noiseCov The system or measurement noise covariance (as square root) @@ -219,36 +225,39 @@ namespace Kalman { * * @return True on success, false if a numerical error is encountered when updating the matrix */ - template - bool computeCovarianceSquareRootFromSigmaPoints(const Type& mean, const SigmaPoints& sigmaPoints, - const CovarianceSquareRoot& noiseCov, CovarianceSquareRoot& cov) - { + template + bool computeCovarianceSquareRootFromSigmaPoints( + const Type & mean, const SigmaPoints & sigmaPoints, + const CovarianceSquareRoot & noiseCov, CovarianceSquareRoot & cov) + { // Compute QR decomposition of (transposed) augmented matrix - Matrix tmp; - tmp.template topRows<2*State::RowsAtCompileTime>() = std::sqrt(this->sigmaWeights_c[1]) * ( sigmaPoints.template rightCols().colwise() - mean).transpose(); - tmp.template bottomRows() = noiseCov.matrixU().toDenseMatrix(); + Matrix tmp; + tmp.template topRows<2 * State::RowsAtCompileTime>() = std::sqrt(this->sigmaWeights_c[1]) * + ( sigmaPoints.template rightCols().colwise() - mean).transpose(); + tmp.template bottomRows() = noiseCov.matrixU().toDenseMatrix(); // TODO: Use ColPivHouseholderQR - Eigen::HouseholderQR qr( tmp ); - + Eigen::HouseholderQR qr(tmp); + // Set R matrix as upper triangular square root - cov.setU(qr.matrixQR().template topRightCorner()); - + cov.setU(qr.matrixQR().template topRightCorner()); + // Perform additional rank 1 update // TODO: According to the paper (Section 3, "Cholesky factor updating") the update // is defined using the square root of the scalar, however the correct result // is obtained when using the weight directly rather than using the square root // It should be checked whether this is a bug in Eigen or in the Paper // T nu = std::copysign( std::sqrt(std::abs(sigmaWeights_c[0])), sigmaWeights_c[0]); - T nu = this->sigmaWeights_c[0]; - cov.rankUpdate( sigmaPoints.template leftCols<1>() - mean, nu ); - - return (cov.info() == Eigen::Success); - } - + T nu = this->sigmaWeights_c[0]; + cov.rankUpdate(sigmaPoints.template leftCols<1>() - mean, nu); + + return cov.info() == Eigen::Success; + } + /** * @brief Compute the Kalman Gain from predicted measurement and sigma points and the innovation covariance. - * + * * @note This covers equations (27) and (28) of Algorithm 3.1 in the Paper * * We need to solve the equation \f$ K (S_y S_y^T) = P \f$ for \f$ K \f$ using backsubstitution. @@ -268,50 +277,51 @@ namespace Kalman { * @param [in] S_y The innovation covariance as square-root * @param [out] K The computed Kalman Gain matrix \f$ K \f$ */ - template - bool computeKalmanGain( const Measurement& y, - const SigmaPoints& sigmaMeasurementPoints, - const CovarianceSquareRoot& S_y, - KalmanGain& K) - { + template + bool computeKalmanGain( + const Measurement & y, + const SigmaPoints & sigmaMeasurementPoints, + const CovarianceSquareRoot & S_y, + KalmanGain & K) + { // Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs // when Measurement::RowsAtCompileTime == 1 AND State::RowsAtCompileTime >= 8 - decltype(sigmaStatePoints) W = this->sigmaWeights_c.transpose().template replicate(); - Matrix P - = (sigmaStatePoints.colwise() - x).cwiseProduct( W ).eval() - * (sigmaMeasurementPoints.colwise() - y).transpose(); - - K = S_y.solve(P.transpose()).transpose(); - return true; - } - + decltype(sigmaStatePoints) W = this->sigmaWeights_c.transpose().template replicate(); + Matrix P = + (sigmaStatePoints.colwise() - x).cwiseProduct(W).eval() * + (sigmaMeasurementPoints.colwise() - y).transpose(); + + K = S_y.solve(P.transpose()).transpose(); + return true; + } + /** * @brief Update the state covariance matrix using the Kalman Gain and the Innovation Covariance - * + * * @note This covers equations (29) and (30) of Algorithm 3.1 in the Paper * * @param [in] K The computed Kalman Gain matrix * @param [in] S_y The innovation covariance as square-root * @return True on success, false if a numerical error is encountered when updating the matrix */ - template - bool updateStateCovariance(const KalmanGain& K, const CovarianceSquareRoot& S_y) - { - KalmanGain U = K * S_y.matrixL(); - for(int i = 0; i < U.cols(); ++i) - { - S.rankUpdate( U.col(i), -1 ); - - if( S.info() == Eigen::NumericalIssue ) - { + template + bool updateStateCovariance( + const KalmanGain & K, + const CovarianceSquareRoot & S_y) + { + KalmanGain U = K * S_y.matrixL(); + for(int i = 0; i < U.cols(); ++i) { + S.rankUpdate(U.col(i), -1); + + if(S.info() == Eigen::NumericalIssue) { // TODO: handle numerical issues in some sensible way - return false; - } - } - - return true; - } - }; + return false; + } + } + + return true; + } +}; } #endif diff --git a/new_super_vision_filter/include/kalman/StandardBase.hpp b/new_super_vision_filter/include/kalman/StandardBase.hpp index 7d96d4b0a..d3b88af9f 100644 --- a/new_super_vision_filter/include/kalman/StandardBase.hpp +++ b/new_super_vision_filter/include/kalman/StandardBase.hpp @@ -24,45 +24,46 @@ #include "Types.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Abstract base class for standard (non-square root) filters and models - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class StandardBase - { - protected: +template +class StandardBase +{ +protected: //! Covariance - Covariance P; - - public: + Covariance P; + +public: /** * Get covariance */ - const Covariance& getCovariance() const - { - return P; - } - + const Covariance & getCovariance() const + { + return P; + } + /** * Get covariance (as square root) */ - CovarianceSquareRoot getCovarianceSquareRoot() const - { - return CovarianceSquareRoot(P); - } - + CovarianceSquareRoot getCovarianceSquareRoot() const + { + return CovarianceSquareRoot(P); + } + /** * Set Covariance */ - bool setCovariance(const Covariance& covariance) - { - P = covariance; - return true; - } + bool setCovariance(const Covariance & covariance) + { + P = covariance; + return true; + } /** * @brief Set Covariance using Square Root @@ -70,21 +71,20 @@ namespace Kalman { * @param covSquareRoot Lower triangular Matrix representing the covariance * square root (i.e. P = LLˆT) */ - bool setCovarianceSquareRoot(const Covariance& covSquareRoot) - { - CovarianceSquareRoot S; - S.setL(covSquareRoot); - P = S.reconstructedMatrix(); - return true; - } + bool setCovarianceSquareRoot(const Covariance & covSquareRoot) + { + CovarianceSquareRoot S; + S.setL(covSquareRoot); + P = S.reconstructedMatrix(); + return true; + } - - protected: - StandardBase() - { - P.setIdentity(); - } - }; +protected: + StandardBase() + { + P.setIdentity(); + } +}; } #endif diff --git a/new_super_vision_filter/include/kalman/StandardFilterBase.hpp b/new_super_vision_filter/include/kalman/StandardFilterBase.hpp index 03336f530..bf2147f40 100644 --- a/new_super_vision_filter/include/kalman/StandardFilterBase.hpp +++ b/new_super_vision_filter/include/kalman/StandardFilterBase.hpp @@ -24,23 +24,24 @@ #include "StandardBase.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Abstract base class for standard (non-square root) filters - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class StandardFilterBase : public StandardBase - { - protected: +template +class StandardFilterBase : public StandardBase +{ +protected: //! Standard Base Type - typedef StandardBase Base; - + typedef StandardBase Base; + //! Covariance matrix - using Base::P; - }; + using Base::P; +}; } #endif diff --git a/new_super_vision_filter/include/kalman/SystemModel.hpp b/new_super_vision_filter/include/kalman/SystemModel.hpp index f20d73c6b..53a4e1ee8 100644 --- a/new_super_vision_filter/include/kalman/SystemModel.hpp +++ b/new_super_vision_filter/include/kalman/SystemModel.hpp @@ -27,7 +27,8 @@ #include "Matrix.hpp" #include "StandardBase.hpp" -namespace Kalman { +namespace Kalman +{ /** * @brief Abstract base class of all system models * @@ -35,35 +36,38 @@ namespace Kalman { * @param ControlType The vector-type of the control input (usually some type derived from Kalman::Vector) * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) */ - template, template class CovarianceBase = StandardBase> - class SystemModel : public CovarianceBase - { - static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/ StateType::RowsAtCompileTime > 0, +template, + template class CovarianceBase = StandardBase> +class SystemModel : public CovarianceBase +{ + static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/ StateType::RowsAtCompileTime > 0, "State vector must contain at least 1 element" /* or be dynamic */); - static_assert(/*ControlType::RowsAtCompileTime == Dynamic ||*/ ControlType::RowsAtCompileTime >= 0, + static_assert(/*ControlType::RowsAtCompileTime == Dynamic ||*/ ControlType::RowsAtCompileTime >= + 0, "Control vector must contain at least 0 elements" /* or be dynamic */); - static_assert(std::is_same::value, + static_assert(std::is_same::value, "State and Control scalar types must be identical"); - public: + +public: //! System state type - typedef StateType State; - + typedef StateType State; + //! System control input type - typedef ControlType Control; - - public: + typedef ControlType Control; + +public: /** * @brief State Transition Function f - * + * * Computes the predicted system state in the next timestep given * the current state x and the control input u */ - virtual State f(const State& x, const Control& u) const = 0; - - protected: - SystemModel() {} - virtual ~SystemModel() {} - }; + virtual State f(const State & x, const Control & u) const = 0; + +protected: + SystemModel() {} + virtual ~SystemModel() {} +}; } #endif diff --git a/new_super_vision_filter/include/kalman/Types.hpp b/new_super_vision_filter/include/kalman/Types.hpp index 38772a01c..0af0cfc3b 100644 --- a/new_super_vision_filter/include/kalman/Types.hpp +++ b/new_super_vision_filter/include/kalman/Types.hpp @@ -32,44 +32,44 @@ namespace Kalman * @param T The numeric scalar type * @param N The dimensionality of the Matrix */ - template - using SquareMatrix = Matrix; - +template +using SquareMatrix = Matrix; + /** * @class Kalman::Covariance * @brief Template type for covariance matrices * @param Type The vector type for which to generate a covariance (usually a state or measurement type) */ - template - using Covariance = SquareMatrix; - +template +using Covariance = SquareMatrix; + /** * @class Kalman::CovarianceSquareRoot * @brief Template type for covariance square roots * @param Type The vector type for which to generate a covariance (usually a state or measurement type) */ - template - using CovarianceSquareRoot = Cholesky< Covariance >; - +template +using CovarianceSquareRoot = Cholesky>; + /** * @class Kalman::KalmanGain * @brief Template type of Kalman Gain * @param State The system state type * @param Measurement The measurement type */ - template - using KalmanGain = Matrix; - +template +using KalmanGain = Matrix; + /** * @class Kalman::Jacobian * @brief Template type of jacobian of A w.r.t. B */ - template - using Jacobian = Matrix; +template +using Jacobian = Matrix; } #endif diff --git a/new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp index cd13a345a..eaffc0084 100644 --- a/new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp +++ b/new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp @@ -25,8 +25,9 @@ #include "UnscentedKalmanFilterBase.hpp" #include "StandardFilterBase.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Unscented Kalman Filter (UKF) * @@ -34,90 +35,91 @@ namespace Kalman { * * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. * Whenever "the paper" is referenced within this file then this paper is meant. - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class UnscentedKalmanFilter : public UnscentedKalmanFilterBase, - public StandardFilterBase - { - public: +template +class UnscentedKalmanFilter : public UnscentedKalmanFilterBase, + public StandardFilterBase +{ +public: //! Unscented Kalman Filter base type - typedef UnscentedKalmanFilterBase UnscentedBase; - + typedef UnscentedKalmanFilterBase UnscentedBase; + //! Standard Filter base type - typedef StandardFilterBase StandardBase; - + typedef StandardFilterBase StandardBase; + //! Numeric Scalar Type inherited from base - using typename UnscentedBase::T; - + using typename UnscentedBase::T; + //! State Type inherited from base - using typename UnscentedBase::State; - + using typename UnscentedBase::State; + //! Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = typename UnscentedBase::template MeasurementModelType; + template class CovarianceBase> + using MeasurementModelType = typename UnscentedBase::template MeasurementModelType; //! System Model Type - template class CovarianceBase> - using SystemModelType = typename UnscentedBase::template SystemModelType; - - protected: + template class CovarianceBase> + using SystemModelType = typename UnscentedBase::template SystemModelType; + +protected: //! The number of sigma points (depending on state dimensionality) - using UnscentedBase::SigmaPointCount; - + using UnscentedBase::SigmaPointCount; + //! Matrix type containing the sigma state or measurement points - template - using SigmaPoints = typename UnscentedBase::template SigmaPoints; - + template + using SigmaPoints = typename UnscentedBase::template SigmaPoints; + //! Kalman Gain Matrix Type - template - using KalmanGain = Kalman::KalmanGain; - - protected: + template + using KalmanGain = Kalman::KalmanGain; + +protected: // Member variables - + //! State Estimate - using UnscentedBase::x; - + using UnscentedBase::x; + //! State Covariance - using StandardBase::P; - + using StandardBase::P; + //! Sigma points (state) - using UnscentedBase::sigmaStatePoints; - - public: + using UnscentedBase::sigmaStatePoints; + +public: /** * Constructor - * + * * See paper for detailed parameter explanation - * + * * @param alpha Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) * @param beta Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) * @param kappa Secondary scaling parameter (usually 0) */ - UnscentedKalmanFilter(T alpha = T(1), T beta = T(2), T kappa = T(0)) - : UnscentedKalmanFilterBase(alpha, beta, kappa) - { + UnscentedKalmanFilter(T alpha = T(1), T beta = T(2), T kappa = T(0)) + : UnscentedKalmanFilterBase(alpha, beta, kappa) + { // Init covariance to identity - P.setIdentity(); - } - + P.setIdentity(); + } + /** * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) * * @param [in] s The System model * @return The updated state estimate */ - template class CovarianceBase> - const State& predict( const SystemModelType& s ) - { + template class CovarianceBase> + const State & predict(const SystemModelType & s) + { // predict state (without control) - Control u; - u.setZero(); - return predict( s, u ); - } - + Control u; + u.setZero(); + return predict(s, u); + } + /** * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model * @@ -125,26 +127,25 @@ namespace Kalman { * @param [in] u The Control input vector * @return The updated state estimate */ - template class CovarianceBase> - const State& predict( const SystemModelType& s, const Control& u ) - { + template class CovarianceBase> + const State & predict(const SystemModelType & s, const Control & u) + { // Compute sigma points - if(!computeSigmaPoints()) - { + if(!computeSigmaPoints()) { // TODO: handle numerical error - assert(false); - } - + assert(false); + } + // Compute predicted state - x = this->template computeStatePrediction(s, u); - + x = this->template computeStatePrediction(s, u); + // Compute predicted covariance - computeCovarianceFromSigmaPoints(x, sigmaStatePoints, s.getCovariance(), P); - + computeCovarianceFromSigmaPoints(x, sigmaStatePoints, s.getCovariance(), P); + // Return predicted state - return this->getState(); - } - + return this->getState(); + } + /** * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model * @@ -152,63 +153,65 @@ namespace Kalman { * @param [in] z The measurement vector * @return The updated state estimate */ - template class CovarianceBase> - const State& update( const MeasurementModelType& m, const Measurement& z ) - { - SigmaPoints sigmaMeasurementPoints; - + template class CovarianceBase> + const State & update( + const MeasurementModelType & m, + const Measurement & z) + { + SigmaPoints sigmaMeasurementPoints; + // Predict measurement (and corresponding sigma points) - Measurement y = this->template computeMeasurementPrediction(m, sigmaMeasurementPoints); - + Measurement y = this->template computeMeasurementPrediction(m, + sigmaMeasurementPoints); + // Compute innovation covariance - Covariance P_yy; - computeCovarianceFromSigmaPoints(y, sigmaMeasurementPoints, m.getCovariance(), P_yy); - - KalmanGain K; - computeKalmanGain(y, sigmaMeasurementPoints, P_yy, K); - + Covariance P_yy; + computeCovarianceFromSigmaPoints(y, sigmaMeasurementPoints, m.getCovariance(), P_yy); + + KalmanGain K; + computeKalmanGain(y, sigmaMeasurementPoints, P_yy, K); + // Update state - x += K * ( z - y ); - + x += K * ( z - y ); + // Update state covariance - updateStateCovariance(K, P_yy); - - return this->getState(); - } - - protected: + updateStateCovariance(K, P_yy); + + return this->getState(); + } + +protected: /** * @brief Compute sigma points from current state estimate and state covariance - * + * * @note This covers equations (6) and (9) of Algorithm 2.1 in the Paper */ - bool computeSigmaPoints() - { + bool computeSigmaPoints() + { // Get square root of covariance - CovarianceSquareRoot llt; - llt.compute(P); - if(llt.info() != Eigen::Success) - { - return false; - } - - SquareMatrix _S = llt.matrixL().toDenseMatrix(); - + CovarianceSquareRoot llt; + llt.compute(P); + if(llt.info() != Eigen::Success) { + return false; + } + + SquareMatrix _S = llt.matrixL().toDenseMatrix(); + // Set left "block" (first column) - sigmaStatePoints.template leftCols<1>() = x; + sigmaStatePoints.template leftCols<1>() = x; // Set center block with x + gamma * S - sigmaStatePoints.template block(0,1) - = ( this->gamma * _S).colwise() + x; + sigmaStatePoints.template block(0, 1) = + ( this->gamma * _S).colwise() + x; // Set right block with x - gamma * S - sigmaStatePoints.template rightCols() - = (-this->gamma * _S).colwise() + x; - - return true; - } - + sigmaStatePoints.template rightCols() = + (-this->gamma * _S).colwise() + x; + + return true; + } + /** * @brief Compute the Covariance from sigma points and noise covariance - * + * * @param [in] mean The mean predicted state or measurement * @param [in] sigmaPoints the predicted sigma state or measurement points * @param [in] noiseCov The system or measurement noise covariance @@ -216,20 +219,21 @@ namespace Kalman { * * @return True on success, false if a numerical error is encountered when updating the matrix */ - template - bool computeCovarianceFromSigmaPoints( const Type& mean, const SigmaPoints& sigmaPoints, - const Covariance& noiseCov, Covariance& cov) - { - decltype(sigmaPoints) W = this->sigmaWeights_c.transpose().template replicate(); - decltype(sigmaPoints) tmp = (sigmaPoints.colwise() - mean); - cov = tmp.cwiseProduct(W) * tmp.transpose() + noiseCov; - - return true; - } - + template + bool computeCovarianceFromSigmaPoints( + const Type & mean, const SigmaPoints & sigmaPoints, + const Covariance & noiseCov, Covariance & cov) + { + decltype(sigmaPoints) W = this->sigmaWeights_c.transpose().template replicate(); + decltype(sigmaPoints) tmp = (sigmaPoints.colwise() - mean); + cov = tmp.cwiseProduct(W) * tmp.transpose() + noiseCov; + + return true; + } + /** * @brief Compute the Kalman Gain from predicted measurement and sigma points and the innovation covariance. - * + * * @note This covers equations (11) and (12) of Algorithm 2.1 in the Paper * * @param [in] y The predicted measurement @@ -237,39 +241,42 @@ namespace Kalman { * @param [in] P_yy The innovation covariance * @param [out] K The computed Kalman Gain matrix \f$ K \f$ */ - template - bool computeKalmanGain( const Measurement& y, - const SigmaPoints& sigmaMeasurementPoints, - const Covariance& P_yy, - KalmanGain& K) - { + template + bool computeKalmanGain( + const Measurement & y, + const SigmaPoints & sigmaMeasurementPoints, + const Covariance & P_yy, + KalmanGain & K) + { // Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs // when Measurement::RowsAtCompileTime == 1 AND State::RowsAtCompileTime >= 8 - decltype(sigmaStatePoints) W = this->sigmaWeights_c.transpose().template replicate(); - Matrix P_xy - = (sigmaStatePoints.colwise() - x).cwiseProduct( W ).eval() - * (sigmaMeasurementPoints.colwise() - y).transpose(); - - K = P_xy * P_yy.inverse(); - return true; - } - + decltype(sigmaStatePoints) W = this->sigmaWeights_c.transpose().template replicate(); + Matrix P_xy = + (sigmaStatePoints.colwise() - x).cwiseProduct(W).eval() * + (sigmaMeasurementPoints.colwise() - y).transpose(); + + K = P_xy * P_yy.inverse(); + return true; + } + /** * @brief Update the state covariance matrix using the Kalman Gain and the Innovation Covariance - * + * * @note This covers equation (14) of Algorithm 2.1 in the Paper * * @param [in] K The computed Kalman Gain matrix * @param [in] P_yy The innovation covariance * @return True on success, false if a numerical error is encountered when updating the matrix */ - template - bool updateStateCovariance(const KalmanGain& K, const Covariance& P_yy) - { - P -= K * P_yy * K.transpose(); - return true; - } - }; + template + bool updateStateCovariance( + const KalmanGain & K, + const Covariance & P_yy) + { + P -= K * P_yy * K.transpose(); + return true; + } +}; } #endif diff --git a/new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp b/new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp index 25e7fb2e9..63795f610 100644 --- a/new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp +++ b/new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp @@ -28,89 +28,91 @@ #include "SystemModel.hpp" #include "MeasurementModel.hpp" -namespace Kalman { - +namespace Kalman +{ + /** * @brief Abstract Base for Unscented Kalman Filters * * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. * Whenever "the paper" is referenced within this file then this paper is meant. - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ - template - class UnscentedKalmanFilterBase : public KalmanFilterBase - { - public: +template +class UnscentedKalmanFilterBase : public KalmanFilterBase +{ +public: // Public typedefs //! Kalman Filter base type - typedef KalmanFilterBase Base; - + typedef KalmanFilterBase Base; + //! Numeric Scalar Type inherited from base - using typename Base::T; - + using typename Base::T; + //! State Type inherited from base - using typename Base::State; - + using typename Base::State; + //! Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = MeasurementModel; - + template class CovarianceBase> + using MeasurementModelType = MeasurementModel; + //! System Model Type - template class CovarianceBase> - using SystemModelType = SystemModel; - - protected: + template class CovarianceBase> + using SystemModelType = SystemModel; + +protected: // Protected typedefs //! The number of sigma points (depending on state dimensionality) - static constexpr int SigmaPointCount = 2 * State::RowsAtCompileTime + 1; - + static constexpr int SigmaPointCount = 2 * State::RowsAtCompileTime + 1; + //! Vector containg the sigma scaling weights - typedef Vector SigmaWeights; - + typedef Vector SigmaWeights; + //! Matrix type containing the sigma state or measurement points - template - using SigmaPoints = Matrix; + template + using SigmaPoints = Matrix; - protected: +protected: // Member variables - + //! State Estimate - using Base::x; - + using Base::x; + //! Sigma weights (m) - SigmaWeights sigmaWeights_m; + SigmaWeights sigmaWeights_m; //! Sigma weights (c) - SigmaWeights sigmaWeights_c; - + SigmaWeights sigmaWeights_c; + //! Sigma points (state) - SigmaPoints sigmaStatePoints; - + SigmaPoints sigmaStatePoints; + // Weight parameters - T alpha; //!< Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) - T beta; //!< Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) - T kappa; //!< Secondary scaling parameter (usually 0) - T gamma; //!< \f$ \gamma = \sqrt{L + \lambda} \f$ with \f$ L \f$ being the state dimensionality - T lambda; //!< \f$ \lambda = \alpha^2 ( L + \kappa ) - L\f$ with \f$ L \f$ being the state dimensionality - - protected: + T alpha; //!< Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) + T beta; //!< Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) + T kappa; //!< Secondary scaling parameter (usually 0) + T gamma; //!< \f$ \gamma = \sqrt{L + \lambda} \f$ with \f$ L \f$ being the state dimensionality + T lambda; //!< \f$ \lambda = \alpha^2 ( L + \kappa ) - L\f$ with \f$ L \f$ being the state dimensionality + +protected: /** * Constructor - * + * * See paper for parameter explanation - * + * * @param _alpha Scaling parameter for spread of sigma points (usually 1e-4 <= alpha <= 1) * @param _beta Parameter for prior knowledge about the distribution (beta = 2 is optimal for Gaussian) * @param _kappa Secondary scaling parameter (usually 0) */ - UnscentedKalmanFilterBase(T _alpha = T(1), T _beta = T(2), T _kappa = T(0)) : alpha(_alpha), beta(_beta), kappa(_kappa) - { + UnscentedKalmanFilterBase(T _alpha = T(1), T _beta = T(2), T _kappa = T(0)) + : alpha(_alpha), beta(_beta), kappa(_kappa) + { // Pre-compute all weights - computeWeights(); - + computeWeights(); + // Setup state and covariance - x.setZero(); - } + x.setZero(); + } /** * @brief Compute predicted state using system model and control input @@ -119,16 +121,18 @@ namespace Kalman { * @param [in] u The Control input * @return The predicted state */ - template class CovarianceBase> - State computeStatePrediction(const SystemModelType& s, const Control& u) - { + template class CovarianceBase> + State computeStatePrediction( + const SystemModelType & s, + const Control & u) + { // Pass each sigma point through non-linear state transition function - computeSigmaPointTransition(s, u); - + computeSigmaPointTransition(s, u); + // Compute predicted state from predicted sigma points - return computePredictionFromSigmaPoints(sigmaStatePoints); - } - + return computePredictionFromSigmaPoints(sigmaStatePoints); + } + /** * @brief Compute predicted measurement using measurement model and predicted sigma measurements * @@ -136,97 +140,100 @@ namespace Kalman { * @param [in] sigmaMeasurementPoints The predicted sigma measurement points * @return The predicted measurement */ - template class CovarianceBase> - Measurement computeMeasurementPrediction(const MeasurementModelType& m, SigmaPoints& sigmaMeasurementPoints) - { + template class CovarianceBase> + Measurement computeMeasurementPrediction( + const MeasurementModelType & m, SigmaPoints & sigmaMeasurementPoints) + { // Predict measurements for each sigma point - computeSigmaPointMeasurements(m, sigmaMeasurementPoints); - + computeSigmaPointMeasurements(m, sigmaMeasurementPoints); + // Predict measurement from sigma measurement points - return computePredictionFromSigmaPoints(sigmaMeasurementPoints); - } - + return computePredictionFromSigmaPoints(sigmaMeasurementPoints); + } + /** * @brief Compute sigma weights */ - void computeWeights() - { - T L = T(State::RowsAtCompileTime); - lambda = alpha * alpha * ( L + kappa ) - L; - gamma = std::sqrt( L + lambda ); - + void computeWeights() + { + T L = T(State::RowsAtCompileTime); + lambda = alpha * alpha * ( L + kappa ) - L; + gamma = std::sqrt(L + lambda); + // Make sure L != -lambda to avoid division by zero - assert( std::abs(L + lambda) > 1e-6 ); - + assert(std::abs(L + lambda) > 1e-6); + // Make sure L != -kappa to avoid division by zero - assert( std::abs(L + kappa) > 1e-6 ); - - T W_m_0 = lambda / ( L + lambda ); - T W_c_0 = W_m_0 + (T(1) - alpha*alpha + beta); - T W_i = T(1) / ( T(2) * alpha*alpha * (L + kappa) ); - + assert(std::abs(L + kappa) > 1e-6); + + T W_m_0 = lambda / ( L + lambda ); + T W_c_0 = W_m_0 + (T(1) - alpha * alpha + beta); + T W_i = T(1) / ( T(2) * alpha * alpha * (L + kappa) ); + // Make sure W_i > 0 to avoid square-root of negative number - assert( W_i > T(0) ); - - sigmaWeights_m[0] = W_m_0; - sigmaWeights_c[0] = W_c_0; - - for(int i = 1; i < SigmaPointCount; ++i) - { - sigmaWeights_m[i] = W_i; - sigmaWeights_c[i] = W_i; - } - } - + assert(W_i > T(0) ); + + sigmaWeights_m[0] = W_m_0; + sigmaWeights_c[0] = W_c_0; + + for(int i = 1; i < SigmaPointCount; ++i) { + sigmaWeights_m[i] = W_i; + sigmaWeights_c[i] = W_i; + } + } + /** * @brief Predict expected sigma states from current sigma states using system model and control input - * + * * @note This covers equation (18) of Algorithm 3.1 in the Paper * * @param [in] s The System Model * @param [in] u The Control input */ - template class CovarianceBase> - void computeSigmaPointTransition(const SystemModelType& s, const Control& u) - { - for( int i = 0; i < SigmaPointCount; ++i ) - { - sigmaStatePoints.col(i) = s.f( sigmaStatePoints.col(i), u ); - } - } - + template class CovarianceBase> + void computeSigmaPointTransition( + const SystemModelType & s, + const Control & u) + { + for( int i = 0; i < SigmaPointCount; ++i ) { + sigmaStatePoints.col(i) = s.f(sigmaStatePoints.col(i), u); + } + } + /** * @brief Predict the expected sigma measurements from predicted sigma states using measurement model - * + * * @note This covers equation (23) of Algorithm 3.1 in the Paper * * @param [in] m The Measurement model * @param [out] sigmaMeasurementPoints The struct of expected sigma measurements to be computed */ - template class CovarianceBase> - void computeSigmaPointMeasurements(const MeasurementModelType& m, SigmaPoints& sigmaMeasurementPoints) - { - for( int i = 0; i < SigmaPointCount; ++i ) - { - sigmaMeasurementPoints.col(i) = m.h( sigmaStatePoints.col(i) ); - } - } - + template class CovarianceBase> + void computeSigmaPointMeasurements( + const MeasurementModelType & m, + SigmaPoints & sigmaMeasurementPoints) + { + for( int i = 0; i < SigmaPointCount; ++i ) { + sigmaMeasurementPoints.col(i) = m.h(sigmaStatePoints.col(i) ); + } + } + /** * @brief Compute state or measurement prediciton from sigma points using pre-computed sigma weights - * + * * @note This covers equations (19) and (24) of Algorithm 3.1 in the Paper * * @param [in] sigmaPoints The computed sigma points of the desired type (state or measurement) * @return The prediction */ - template - Type computePredictionFromSigmaPoints(const SigmaPoints& sigmaPoints) - { + template + Type computePredictionFromSigmaPoints(const SigmaPoints & sigmaPoints) + { // Use efficient matrix x vector computation - return sigmaPoints * sigmaWeights_m; - } - }; + return sigmaPoints * sigmaWeights_m; + } +}; } #endif diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 189834b08..848fe5a7d 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -32,7 +32,7 @@ class FilteredRobot { public: - FilteredRobot(const RobotTrack & track, ateam_common::TeamColor &team_color); + FilteredRobot(const RobotTrack & track, ateam_common::TeamColor & team_color); void update(const RobotTrack & track); diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp index 28f701f3e..28838c854 100644 --- a/new_super_vision_filter/test/filter_test.cpp +++ b/new_super_vision_filter/test/filter_test.cpp @@ -22,6 +22,8 @@ #include #include #include +#include +#include #include #include @@ -30,11 +32,25 @@ #include "filtered_robot.hpp" #include "measurements/robot_track.hpp" +namespace ateam_msgs +{ +namespace msg +{ +void PrintTo(const VisionStateRobot & state, std::ostream * os) +{ + *os << "\n" << "VisionStateRobot: \n" << + "x pos: " << state.pose.position.x << " y pos: " << state.pose.position.y << + " z pos: " << state.pose.position.z << "\n" << "visible? " << state.visible << + "\n" << "orientation: " << state.pose.orientation.w; +} +} +} + class FilteredRobotTest : public ::testing::Test { protected: ateam_common::TeamColor team = ateam_common::TeamColor::Blue; - int oldEnoughAge = 3; + int oldEnoughAge = 2; ssl_league_msgs::msg::VisionDetectionRobot robot_msg{}; std::unique_ptr bot; @@ -50,26 +66,26 @@ TEST_F(FilteredRobotTest, WaitUntilOldEnough) { ateam_msgs::msg::VisionStateRobot default_msg{}; int camera = 0; - for (size_t i = 0; i < (oldEnoughAge + 1); ++i){ + for (size_t i = 0; i < oldEnoughAge; ++i) { // Set it up so we can print our messages more effectively // https://google.github.io/googletest/advanced.html#teaching-googletest-how-to-print-your-values - ssl_league_msgs::msg::VisionDetectionRobot fake_vision_data{}; - fake_vision_data.pose.position.x = 1; - fake_vision_data.pose.position.y = 1; - if (i < oldEnoughAge) { - auto fake_track = RobotTrack(fake_vision_data, camera, team); - bot->update(fake_track); - auto msg = bot->toMsg(); + ssl_league_msgs::msg::VisionDetectionRobot fake_vision_data{}; + fake_vision_data.pose.position.x = 1; + fake_vision_data.pose.position.y = 1; + if (i < oldEnoughAge) { + auto fake_track = RobotTrack(fake_vision_data, camera, team); + bot->update(fake_track); + auto msg = bot->toMsg(); // We shouldn't update if our filter is too new - EXPECT_EQ(default_msg, msg); - } else { - // Sorry, adding a short sleep was easier than mocking the timestamp... - std::this_thread::sleep_for(std::chrono::milliseconds(60)); - auto fake_track = RobotTrack(fake_vision_data, camera, team); - bot->update(fake_track); - auto msg = bot->toMsg(); - // We should update if our filter is old enough - EXPECT_NE(default_msg, msg); - } + EXPECT_EQ(default_msg, msg); + } else { + // Sorry, adding a short sleep was easier than mocking the timestamp... + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + auto fake_track = RobotTrack(fake_vision_data, camera, team); + bot->update(fake_track); + auto msg = bot->toMsg(); + // We should update if our filter is old enough + EXPECT_NE(default_msg, msg); + } } } From 11c3b7c976e85374980694687702fef212c60378 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 3 Mar 2026 21:58:48 -0500 Subject: [PATCH 29/35] Bring up file for new vision with sim --- .../launch/autonomy_new_vision.launch.xml | 28 +++++ .../launch/new_vision_bringup.launch.py | 104 ++++++++++++++++++ .../src/vision_filter_node.cpp | 4 + 3 files changed, 136 insertions(+) create mode 100644 ateam_bringup/launch/autonomy_new_vision.launch.xml create mode 100644 ateam_bringup/launch/new_vision_bringup.launch.py diff --git a/ateam_bringup/launch/autonomy_new_vision.launch.xml b/ateam_bringup/launch/autonomy_new_vision.launch.xml new file mode 100644 index 000000000..6423da7e6 --- /dev/null +++ b/ateam_bringup/launch/autonomy_new_vision.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ateam_bringup/launch/new_vision_bringup.launch.py b/ateam_bringup/launch/new_vision_bringup.launch.py new file mode 100644 index 000000000..c0e67048b --- /dev/null +++ b/ateam_bringup/launch/new_vision_bringup.launch.py @@ -0,0 +1,104 @@ +# Copyright 2021 A Team +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from ateam_bringup.substitutions import PackageLaunchFileSubstitution +import launch +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import FrontendLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return launch.LaunchDescription([ + DeclareLaunchArgument('start_sim', default_value='True'), + DeclareLaunchArgument('start_gc', default_value='True'), + DeclareLaunchArgument('start_ui', default_value='True'), + DeclareLaunchArgument('headless_sim', default_value='True'), + DeclareLaunchArgument('sim_radio_ip', default_value='127.0.0.1'), + DeclareLaunchArgument('team_name', default_value='A-Team'), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution('ateam_bringup', + 'ssl_erforce_sim.launch.xml')), + launch_arguments={ + 'headless': LaunchConfiguration('headless_sim') + }.items(), + condition=IfCondition(LaunchConfiguration('start_sim')) + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution('ateam_bringup', + 'ssl_game_controller.launch.xml')), + condition=IfCondition(LaunchConfiguration('start_gc')) + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution('ateam_bringup', + 'league_bridges.launch.xml')), + launch_arguments={ + 'gc_net_interface_address': '172.17.0.1', + 'gc_ip_address': '172.17.0.2', + 'vision_net_interface_address': '127.0.0.1', + 'vision_port': '10020', + 'team_name': LaunchConfiguration('team_name') + }.items() + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution('ateam_bringup', + 'autonomy.launch.xml')), + launch_arguments={ + 'team_name': LaunchConfiguration('team_name'), + 'use_emulated_ballsense': 'False', + 'vision_offset_robot_x': '0.0', + 'vision_offset_robot_y': '0.0', + }.items() + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution('ateam_bringup', + 'ui.launch.xml')), + condition=IfCondition(LaunchConfiguration('start_ui')) + ), + + Node( + package='ateam_ssl_simulation_radio_bridge', + executable='ssl_simulation_radio_bridge_node', + name='radio_bridge', + parameters=[{ + 'ssl_sim_radio_ip': LaunchConfiguration('sim_radio_ip'), + 'gc_team_name': LaunchConfiguration('team_name') + }] + ), + + IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PackageLaunchFileSubstitution('ateam_joystick_control', + 'joystick_controller.launch.xml') + ) + ) + ]) diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index b5f502555..ade78c415 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -53,6 +53,10 @@ class VisionFilterNode : public rclcpp::Node : rclcpp::Node("new_ateam_vision_filter", options), game_controller_listener_(*this) { + declare_parameters("offsets", { + {"robots.x", 0.0}, + {"robots.y", 0.0} + }); ssl_vision_subscription_ = create_subscription( std::string(Topics::kVisionMessages), From d1840c59ddb8e783beb6762b393d64c4b19874f9 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 3 Mar 2026 22:33:05 -0500 Subject: [PATCH 30/35] Move kalman to git submodule --- .gitmodules | 3 + new_super_vision_filter/CMakeLists.txt | 23 +- .../include/kalman/ExtendedKalmanFilter.hpp | 156 --------- .../include/kalman/KalmanFilterBase.hpp | 82 ----- .../kalman/LinearizedMeasurementModel.hpp | 84 ----- .../include/kalman/LinearizedSystemModel.hpp | 84 ----- .../include/kalman/Matrix.hpp | 164 --------- .../include/kalman/MeasurementModel.hpp | 72 ---- .../include/kalman/SquareRootBase.hpp | 88 ----- .../kalman/SquareRootExtendedKalmanFilter.hpp | 286 --------------- .../include/kalman/SquareRootFilterBase.hpp | 47 --- .../SquareRootUnscentedKalmanFilter.hpp | 327 ------------------ .../include/kalman/StandardBase.hpp | 90 ----- .../include/kalman/StandardFilterBase.hpp | 47 --- .../include/kalman/SystemModel.hpp | 73 ---- .../include/kalman/Types.hpp | 75 ---- .../include/kalman/UnscentedKalmanFilter.hpp | 282 --------------- .../kalman/UnscentedKalmanFilterBase.hpp | 239 ------------- new_super_vision_filter/kalman | 1 + 19 files changed, 22 insertions(+), 2201 deletions(-) delete mode 100644 new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp delete mode 100644 new_super_vision_filter/include/kalman/KalmanFilterBase.hpp delete mode 100644 new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp delete mode 100644 new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp delete mode 100644 new_super_vision_filter/include/kalman/Matrix.hpp delete mode 100644 new_super_vision_filter/include/kalman/MeasurementModel.hpp delete mode 100644 new_super_vision_filter/include/kalman/SquareRootBase.hpp delete mode 100644 new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp delete mode 100644 new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp delete mode 100644 new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp delete mode 100644 new_super_vision_filter/include/kalman/StandardBase.hpp delete mode 100644 new_super_vision_filter/include/kalman/StandardFilterBase.hpp delete mode 100644 new_super_vision_filter/include/kalman/SystemModel.hpp delete mode 100644 new_super_vision_filter/include/kalman/Types.hpp delete mode 100644 new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp delete mode 100644 new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp create mode 160000 new_super_vision_filter/kalman diff --git a/.gitmodules b/.gitmodules index 08ea60349..015d9ec6a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "radio/ateam_radio_msgs/software-communication"] path = radio/ateam_radio_msgs/software-communication url = https://github.com/SSL-A-Team/software-communication.git +[submodule "new_super_vision_filter/kalman"] + path = new_super_vision_filter/kalman + url = https://github.com/mherb/kalman.git diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt index 652ca5c3a..a95ad9db3 100644 --- a/new_super_vision_filter/CMakeLists.txt +++ b/new_super_vision_filter/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(ateam_common REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -add_library(${PROJECT_NAME} SHARED +add_library(${PROJECT_NAME} SHARED src/vision_filter_node.cpp src/camera.cpp src/filtered_robot.cpp @@ -23,7 +23,7 @@ add_library(${PROJECT_NAME} SHARED target_include_directories(${PROJECT_NAME} PUBLIC - $ + $ $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src @@ -42,7 +42,7 @@ rclcpp_components_register_node( PLUGIN "new_super_vision::VisionFilterNode" EXECUTABLE ateam_new_super_vision ) -install(TARGETS ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -51,7 +51,7 @@ install(TARGETS ${PROJECT_NAME} ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) find_package(ament_cmake_gtest REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files @@ -60,7 +60,20 @@ if(BUILD_TESTING) # comment the line when this package is in a git repo and when # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE AMENT_LINT_AUTO_FILE_EXCLUDE + "kalman/*" + "kalman/cmake/*" + ) + file(GLOB_RECURSE ALL_CMAKE_FILES + "${CMAKE_CURRENT_SOURCE_DIR}/*.cmake" + "${CMAKE_CURRENT_SOURCE_DIR}/CMakeLists.txt" + ) + + list(FILTER ALL_CMAKE_FILES EXCLUDE REGEX + ".*/kalman/.*" + ) + + ament_lint_cmake(${ALL_CMAKE_FILES}) add_subdirectory(test) endif() diff --git a/new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp deleted file mode 100644 index 2036307f6..000000000 --- a/new_super_vision_filter/include/kalman/ExtendedKalmanFilter.hpp +++ /dev/null @@ -1,156 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_EXTENDEDKALMANFILTER_HPP_ -#define KALMAN_EXTENDEDKALMANFILTER_HPP_ - -#include "KalmanFilterBase.hpp" -#include "StandardFilterBase.hpp" -#include "LinearizedSystemModel.hpp" -#include "LinearizedMeasurementModel.hpp" - -namespace Kalman -{ - - /** - * @brief Extended Kalman Filter (EKF) - * - * This implementation is based upon [An Introduction to the Kalman Filter](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) - * by Greg Welch and Gary Bishop. - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class ExtendedKalmanFilter : public KalmanFilterBase, - public StandardFilterBase -{ -public: - //! Kalman Filter base type - typedef KalmanFilterBase KalmanBase; - //! Standard Filter base type - typedef StandardFilterBase StandardBase; - - //! Numeric Scalar Type inherited from base - using typename KalmanBase::T; - - //! State Type inherited from base - using typename KalmanBase::State; - - //! Linearized Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = LinearizedMeasurementModel; - - //! Linearized System Model Type - template class CovarianceBase> - using SystemModelType = LinearizedSystemModel; - -protected: - //! Kalman Gain Matrix Type - template - using KalmanGain = Kalman::KalmanGain; - -protected: - //! State Estimate - using KalmanBase::x; - //! State Covariance Matrix - using StandardBase::P; - -public: - /** - * @brief Constructor - */ - ExtendedKalmanFilter() - { - // Setup state and covariance - P.setIdentity(); - } - - /** - * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) - * - * @param [in] s The System model - * @return The updated state estimate - */ - template class CovarianceBase> - const State & predict(SystemModelType & s) - { - // predict state (without control) - Control u; - u.setZero(); - return predict(s, u); - } - - /** - * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model - * - * @param [in] s The System model - * @param [in] u The Control input vector - * @return The updated state estimate - */ - template class CovarianceBase> - const State & predict(SystemModelType & s, const Control & u) - { - s.updateJacobians(x, u); - - // predict state - x = s.f(x, u); - - // predict covariance - P = ( s.F * P * s.F.transpose() ) + ( s.W * s.getCovariance() * s.W.transpose() ); - - // return state prediction - return this->getState(); - } - - /** - * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model - * - * @param [in] m The Measurement model - * @param [in] z The measurement vector - * @return The updated state estimate - */ - template class CovarianceBase> - const State & update(MeasurementModelType & m, const Measurement & z) - { - m.updateJacobians(x); - - // COMPUTE KALMAN GAIN - // compute innovation covariance - Covariance S = ( m.H * P * m.H.transpose() ) + - ( m.V * m.getCovariance() * m.V.transpose() ); - - // compute kalman gain - KalmanGain K = P * m.H.transpose() * S.inverse(); - - // UPDATE STATE ESTIMATE AND COVARIANCE - // Update state using computed kalman gain and innovation - x += K * ( z - m.h(x) ); - - // Update covariance - P -= K * m.H * P; - - // return updated state estimate - return this->getState(); - } -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/KalmanFilterBase.hpp b/new_super_vision_filter/include/kalman/KalmanFilterBase.hpp deleted file mode 100644 index b24dfa0e8..000000000 --- a/new_super_vision_filter/include/kalman/KalmanFilterBase.hpp +++ /dev/null @@ -1,82 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_KALMANFILTERBASE_HPP_ -#define KALMAN_KALMANFILTERBASE_HPP_ - -#include "Matrix.hpp" -#include "Types.hpp" - -namespace Kalman -{ - - /** - * @brief Abstract base class for all Kalman Filters - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class KalmanFilterBase -{ -public: - static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/StateType::RowsAtCompileTime > 0, - "State vector must contain at least 1 element" /* or be dynamic */); - static_assert(StateType::ColsAtCompileTime == 1, "State type must be a column vector"); - - //! Numeric scalar type - typedef typename StateType::Scalar T; - - //! Type of the state vector - typedef StateType State; - -protected: - //! Estimated state - State x; - -public: - /** - * Get current state estimate - */ - const State & getState() const - { - return x; - } - - /** - * @brief Initialize state - * @param initialState The initial state of the system - */ - void init(const State & initialState) - { - x = initialState; - } - -protected: - /** - * @brief Protected constructor - */ - KalmanFilterBase() - { - } -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp b/new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp deleted file mode 100644 index e3d4c3ebf..000000000 --- a/new_super_vision_filter/include/kalman/LinearizedMeasurementModel.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_LINEARIZEDMEASUREMENTMODEL_HPP_ -#define KALMAN_LINEARIZEDMEASUREMENTMODEL_HPP_ - -#include "MeasurementModel.hpp" - -namespace Kalman -{ -template -class ExtendedKalmanFilter; -template -class SquareRootExtendedKalmanFilter; - - /** - * @brief Abstract base class of all linearized (first order taylor expansion) measurement models - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - * @param MeasurementType The vector-type of the measurement (usually some type derived from Kalman::Vector) - * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) - */ -template class CovarianceBase = StandardBase> -class LinearizedMeasurementModel : public MeasurementModel -{ - friend class ExtendedKalmanFilter; - friend class SquareRootExtendedKalmanFilter; - -public: - //! Measurement model base - typedef MeasurementModel Base; - - //! System state type - using typename Base::State; - - //! Measurement vector type - using typename Base::Measurement; - -protected: - //! Measurement model jacobian - Jacobian H; - //! Measurement model noise jacobian - Jacobian V; - - /** - * Callback function for state-dependent update of Jacobi-matrices H and V before each update step - */ - virtual void updateJacobians(const State & x) - { - // No update by default - (void)x; - } - -protected: - LinearizedMeasurementModel() - { - H.setIdentity(); - V.setIdentity(); - } - ~LinearizedMeasurementModel() {} -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp b/new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp deleted file mode 100644 index 1cc197058..000000000 --- a/new_super_vision_filter/include/kalman/LinearizedSystemModel.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_LINEARIZEDSYSTEMMODEL_HPP_ -#define KALMAN_LINEARIZEDSYSTEMMODEL_HPP_ - -#include "SystemModel.hpp" - -namespace Kalman -{ -template -class ExtendedKalmanFilter; -template -class SquareRootExtendedKalmanFilter; - - /** - * @brief Abstract base class of all linearized (first order taylor expansion) system models - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - * @param ControlType The vector-type of the control input (usually some type derived from Kalman::Vector) - * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) - */ -template, - template class CovarianceBase = StandardBase> -class LinearizedSystemModel : public SystemModel -{ - friend class ExtendedKalmanFilter; - friend class SquareRootExtendedKalmanFilter; - -public: - //! System model base - typedef SystemModel Base; - - //! System state type - using typename Base::State; - - //! System control input type - using typename Base::Control; - -protected: - //! System model jacobian - Jacobian F; - //! System model noise jacobian - Jacobian W; - - /** - * Callback function for state-dependent update of Jacobi-matrices F and W before each update step - */ - virtual void updateJacobians(const State & x, const Control & u) - { - // No update by default - (void)x; - (void)u; - } - -protected: - LinearizedSystemModel() - { - F.setIdentity(); - W.setIdentity(); - } - ~LinearizedSystemModel() {} -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/Matrix.hpp b/new_super_vision_filter/include/kalman/Matrix.hpp deleted file mode 100644 index 16c03ee46..000000000 --- a/new_super_vision_filter/include/kalman/Matrix.hpp +++ /dev/null @@ -1,164 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_MATRIX_HPP_ -#define KALMAN_MATRIX_HPP_ - -#include - -#include - -#define KALMAN_VECTOR(NAME, T, N) \ - typedef Kalman::Vector Base; \ - using typename Base::Scalar; \ - using Base::RowsAtCompileTime; \ - using Base::ColsAtCompileTime; \ - using Base::SizeAtCompileTime; \ - \ - NAME(void) : Kalman::Vector() {} \ - \ - template \ - NAME(const Eigen::MatrixBase & other) : Kalman::Vector(other) {} \ - \ - template \ - NAME & operator=(const Eigen::MatrixBase & other) \ - { \ - this->Base::operator=(other); \ - return *this; \ - } - -namespace Kalman -{ -const int Dynamic = Eigen::Dynamic; - - /** - * @class Kalman::Matrix - * @brief Template type for matrices - * @param T The numeric scalar type - * @param rows The number of rows - * @param cols The number of columns - */ -template -using Matrix = Eigen::Matrix; - - /** - * @brief Template type for vectors - * @param T The numeric scalar type - * @param N The vector dimension - */ -template -class Vector : public Matrix -{ -public: - //! Matrix base type - typedef Matrix Base; - - using typename Base::Scalar; - using Base::RowsAtCompileTime; - using Base::ColsAtCompileTime; - using Base::SizeAtCompileTime; - - Vector(void) - : Matrix() {} - - /** - * @brief Copy constructor - */ - template - Vector(const Eigen::MatrixBase & other) - : Matrix(other) - {} - /** - * @brief Copy assignment constructor - */ - template - Vector & operator=(const Eigen::MatrixBase & other) - { - this->Base::operator=(other); - return *this; - } -}; - - /** - * @brief Cholesky square root decomposition of a symmetric positive-definite matrix - * @param _MatrixType The matrix type - * @param _UpLo Square root form (Eigen::Lower or Eigen::Upper) - */ -template -class Cholesky : public Eigen::LLT<_MatrixType, _UpLo> -{ -public: - Cholesky() - : Eigen::LLT<_MatrixType, _UpLo>() {} - - /** - * @brief Construct cholesky square root decomposition from matrix - * @param m The matrix to be decomposed - */ - Cholesky(const _MatrixType & m) - : Eigen::LLT<_MatrixType, _UpLo>(m) {} - - /** - * @brief Set decomposition to identity - */ - Cholesky & setIdentity() - { - this->m_matrix.setIdentity(); - this->m_isInitialized = true; - return *this; - } - - /** - * @brief Check whether the decomposed matrix is the identity matrix - */ - bool isIdentity() const - { - eigen_assert(this->m_isInitialized && "LLT is not initialized."); - return this->m_matrix.isIdentity(); - } - - /** - * @brief Set lower triangular part of the decomposition - * @param matrix The lower part stored in a full matrix - */ - template - Cholesky & setL(const Eigen::MatrixBase & matrix) - { - this->m_matrix = matrix.template triangularView(); - this->m_isInitialized = true; - return *this; - } - - /** - * @brief Set upper triangular part of the decomposition - * @param matrix The upper part stored in a full matrix - */ - template - Cholesky & setU(const Eigen::MatrixBase & matrix) - { - this->m_matrix = matrix.template triangularView().adjoint(); - this->m_isInitialized = true; - return *this; - } -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/MeasurementModel.hpp b/new_super_vision_filter/include/kalman/MeasurementModel.hpp deleted file mode 100644 index f36f1a1c2..000000000 --- a/new_super_vision_filter/include/kalman/MeasurementModel.hpp +++ /dev/null @@ -1,72 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_MEASUREMENTMODEL_HPP_ -#define KALMAN_MEASUREMENTMODEL_HPP_ - -#include - -#include "StandardBase.hpp" - -namespace Kalman -{ - /** - * @brief Abstract base class of all measurement models - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - * @param MeasurementType The vector-type of the measurement (usually some type derived from Kalman::Vector) - * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) - */ -template class CovarianceBase = StandardBase> -class MeasurementModel : public CovarianceBase -{ - static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/StateType::RowsAtCompileTime > 0, - "State vector must contain at least 1 element" /* or be dynamic */); - static_assert( - /*MeasurementType::RowsAtCompileTime == Dynamic ||*/MeasurementType:: - RowsAtCompileTime > 0, - "Measurement vector must contain at least 1 element" /* or be dynamic */); - static_assert(std::is_same::value, - "State and Measurement scalar types must be identical"); - -public: - //! System state type - typedef StateType State; - - //! Measurement vector type - typedef MeasurementType Measurement; - -public: - /** - * Measurement Model Function h - * - * Predicts the estimated measurement value given the current state estimate x - */ - virtual Measurement h(const State & x) const = 0; - -protected: - MeasurementModel() {} - virtual ~MeasurementModel() {} -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/SquareRootBase.hpp b/new_super_vision_filter/include/kalman/SquareRootBase.hpp deleted file mode 100644 index 40ecd6a94..000000000 --- a/new_super_vision_filter/include/kalman/SquareRootBase.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_SQUAREROOTBASE_HPP_ -#define KALMAN_SQUAREROOTBASE_HPP_ - -#include "Types.hpp" - -namespace Kalman -{ - - /** - * @brief Abstract base class for square-root filters and models - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class SquareRootBase -{ -protected: - //! Covariance Square Root - CovarianceSquareRoot S; - -public: - /** - * Get covariance (as square root) - */ - const CovarianceSquareRoot & getCovarianceSquareRoot() const - { - return S; - } - - /** - * Get covariance reconstructed from square root - */ - Covariance getCovariance() const - { - return S.reconstructedMatrix(); - } - - /** - * Set Covariance - */ - bool setCovariance(const Covariance & covariance) - { - S.compute(covariance); - return S.info() == Eigen::Success; - } - - /** - * @brief Set Covariance using Square Root - * - * @param covSquareRoot Lower triangular Matrix representing the covariance - * square root (i.e. P = LLˆT) - */ - bool setCovarianceSquareRoot(const Covariance & covSquareRoot) - { - S.setL(covSquareRoot); - return true; - } - -protected: - SquareRootBase() - { - S.setIdentity(); - } -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp deleted file mode 100644 index 8f0a10d95..000000000 --- a/new_super_vision_filter/include/kalman/SquareRootExtendedKalmanFilter.hpp +++ /dev/null @@ -1,286 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_SQUAREROOTEXTENDEDKALMANFILTER_HPP_ -#define KALMAN_SQUAREROOTEXTENDEDKALMANFILTER_HPP_ - -#include "KalmanFilterBase.hpp" -#include "SquareRootFilterBase.hpp" -#include "LinearizedSystemModel.hpp" -#include "LinearizedMeasurementModel.hpp" - -namespace Kalman -{ - - /** - * @brief Square-Root Extended Kalman Filter (SR-EKF) - * - * This implementation is based upon [An Introduction to the Kalman Filter](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) - * by Greg Welch and Gary Bishop. - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class SquareRootExtendedKalmanFilter : public KalmanFilterBase, - public SquareRootFilterBase -{ -public: - //! Kalman Filter base type - typedef KalmanFilterBase KalmanBase; - //! SquareRoot Filter base type - typedef SquareRootFilterBase SquareRootBase; - - //! Numeric Scalar Type inherited from base - using typename KalmanBase::T; - - //! State Type inherited from base - using typename KalmanBase::State; - - //! Linearized Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = LinearizedMeasurementModel; - - //! Linearized System Model Type - template class CovarianceBase> - using SystemModelType = LinearizedSystemModel; - -protected: - //! Kalman Gain Matrix Type - template - using KalmanGain = Kalman::KalmanGain; - -protected: - //! State estimate - using KalmanBase::x; - //! State covariance square root - using SquareRootBase::S; - -public: - /** - * @brief Constructor - */ - SquareRootExtendedKalmanFilter() - { - // Setup covariance - S.setIdentity(); - } - - /** - * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) - * - * @param [in] s The System model - * @return The updated state estimate - */ - template class CovarianceBase> - const State & predict(SystemModelType & s) - { - // predict state (without control) - Control u; - u.setZero(); - return predict(s, u); - } - - /** - * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model - * - * @param [in] s The System model - * @param [in] u The Control input vector - * @return The updated state estimate - */ - template class CovarianceBase> - const State & predict(SystemModelType & s, const Control & u) - { - s.updateJacobians(x, u); - - // predict state - x = s.f(x, u); - - // predict covariance - computePredictedCovarianceSquareRoot(s.F, S, s.W, s.getCovarianceSquareRoot(), S); - - // return state prediction - return this->getState(); - } - - /** - * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model - * - * @param [in] m The Measurement model - * @param [in] z The measurement vector - * @return The updated state estimate - */ - template class CovarianceBase> - const State & update(MeasurementModelType & m, const Measurement & z) - { - m.updateJacobians(x); - - // COMPUTE KALMAN GAIN - // compute innovation covariance - CovarianceSquareRoot S_y; - computePredictedCovarianceSquareRoot(m.H, S, m.V, m.getCovarianceSquareRoot(), - S_y); - - // compute kalman gain - KalmanGain K; - computeKalmanGain(m.H, S_y, K); - - // UPDATE STATE ESTIMATE AND COVARIANCE - // Update state using computed kalman gain and innovation - x += K * ( z - m.h(x) ); - - // Update covariance - updateStateCovariance(K, m.H); - - // return updated state estimate - return this->getState(); - } - -protected: - /** - * @brief Compute the predicted state or innovation covariance (as square root) - * - * The formula for computing the propagated square root covariance matrix can be - * deduced in a very straight forward way using the method outlined in the - * [Unscented Kalman Filter Tutorial](https://cse.sc.edu/~terejanu/files/tutorialUKF.pdf) - * by Gabriel A. Terejanu for the UKF (Section 3, formulas (27) and (28)). - * - * Starting from the standard update formula - * - * \f[ \hat{P} = FPF^T + WQW^T \f] - * - * and using the square-root decomposition \f$ P = SS^T \f$ with \f$S\f$ being lower-triagonal - * as well as the (lower triangular) square root \f$\sqrt{Q}\f$ of \f$Q\f$ this can be formulated as - * - * \f{align*}{ - * \hat{P} &= FSS^TF^T + W\sqrt{Q}\sqrt{Q}^TW^T \\ - * &= FS(FS)^T + W\sqrt{Q}(W\sqrt{Q})^T \\ - * &= \begin{bmatrix} FS & W\sqrt{Q} \end{bmatrix} - * \begin{bmatrix} (FS)^T \\ (W\sqrt{Q})^T \end{bmatrix} - * \f} - * - * The blockmatrix - * - * \f[ \begin{bmatrix} (FS)^T \\ (W\sqrt{Q})^T \end{bmatrix} \in \mathbb{R}^{2n \times n} \f] - * - * can then be decomposed into a product of matrices \f$OR\f$ with \f$O\f$ being orthogonal - * and \f$R\f$ being upper triangular (also known as QR decompositon). Using this \f$\hat{P}\f$ - * can be written as - * - * \f{align*}{ - * \hat{P} &= \begin{bmatrix} FS & W\sqrt{Q} \end{bmatrix} - * \begin{bmatrix} (FS)^T \\ (W\sqrt{Q})^T \end{bmatrix} \\ - * &= (OR)^TOR \\ - * &= R^T \underbrace{O^T O}_{=I}R \\ - * &= LL^T \qquad \text{ with } L := R^T - * \f} - * - * Thus the lower-triangular square root of \f$\hat{P}\f$ is equaivalent to the transpose - * of the upper-triangular matrix obtained from QR-decompositon of the augmented block matrix - * - * \f[ \begin{bmatrix} FS & W\sqrt{Q} \end{bmatrix}^T = \begin{bmatrix} S^T F^T \\ \sqrt{Q}^T W^T \end{bmatrix} \in \mathbb{R}^{2n \times n} \f] - * - * The same can be applied for the innovation covariance by replacing the jacobians and - * the noise covariance accordingly. - * - * @param [in] A The jacobian of state transition or measurement function w.r.t. state or measurement - * @param [in] S The state covariance (as square root) - * @param [in] B The jacobian of state transition or measurement function w.r.t. state or measurement - * @param [in] R The system model or measurement noise covariance (as square root) - * @param [out] S_pred The predicted covariance (as square root) - * @return True on success, false on failure due to numerical issue - */ - template - bool computePredictedCovarianceSquareRoot( - const Jacobian & A, const CovarianceSquareRoot & S, - const Jacobian & B, const CovarianceSquareRoot & R, - CovarianceSquareRoot & S_pred) - { - // Compute QR decomposition of (transposed) augmented matrix - Matrix tmp; - tmp.template topRows() = S.matrixU() * A.transpose(); - tmp.template bottomRows() = R.matrixU() * B.transpose(); - - // TODO: Use ColPivHouseholderQR - Eigen::HouseholderQR qr(tmp); - - // Set S_pred matrix as upper triangular square root - S_pred.setU(qr.matrixQR().template topRightCorner()); - return true; - } - - /** - * @brief Compute Kalman gain from state covariance, innovation covariance and measurement jacobian - * - * The formula for the kalman gain using square-root covariances can be deduced from the - * original update formula - * - * \f[ K = P H^T P_{yy}^{-1} \Leftrightarrow K P_{yy} = P H^T \f] - * - * with \f$P_yy\f$ being the innovation covariance. With \f$P = SS^T\f$ and \f$P_yy = S_yS_y^T\f$ - * and transposition of the whole equation the above can be formulated as - * - * \f{align*}{ - * K (S_yS_y^T) &= SS^TH^T \\ - * (K(S_yS_y^T))^T &= (SS^TH^T)^T \\ - * (S_yS_y^T)^TK^T &= H(SS^T)^T \\ - * \underbrace{S_yS_y^T}_{=P_{yy}} K^T &= HSS^T - * \f} - * - * Since \f$S_y\f$ is lower triangular, the above can be solved using backsubstitution - * in an efficient manner. - * - * @param [in] H The jacobian of the measurement function w.r.t. the state - * @param [in] S_y The Innovation covariance square root - * @param [out] K The computed Kalman Gain - */ - template - bool computeKalmanGain( - const Jacobian & H, - const CovarianceSquareRoot & S_y, - KalmanGain & K) - { - // Solve using backsubstitution - // AX=B with B = HSS^T and X = K^T and A = S_yS_y^T - K = S_y.solve(H * S.reconstructedMatrix()).transpose(); - return true; - } - - /** - * @brief Update state covariance using Kalman gain - * - * @param [in] K The Kalman gain - * @param [in] H The jacobian of the measurement function w.r.t. the state - */ - template - bool updateStateCovariance( - const KalmanGain & K, - const Jacobian & H) - { - // TODO: update covariance without using decomposition - Matrix P = S.reconstructedMatrix(); - S.compute( (P - K * H * P).eval() ); - return S.info() == Eigen::Success; - } -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp b/new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp deleted file mode 100644 index b53641889..000000000 --- a/new_super_vision_filter/include/kalman/SquareRootFilterBase.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_SQUAREROOTFILTERBASE_HPP_ -#define KALMAN_SQUAREROOTFILTERBASE_HPP_ - -#include "SquareRootBase.hpp" - -namespace Kalman -{ - - /** - * @brief Abstract base class for square root filters - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class SquareRootFilterBase : public SquareRootBase -{ -protected: - //! SquareRoot Base Type - typedef SquareRootBase Base; - - //! Covariance Square Root - using Base::S; -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp deleted file mode 100644 index c032b9805..000000000 --- a/new_super_vision_filter/include/kalman/SquareRootUnscentedKalmanFilter.hpp +++ /dev/null @@ -1,327 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_SQUAREROOTUNSCENTEDKALMANFILTER_HPP_ -#define KALMAN_SQUAREROOTUNSCENTEDKALMANFILTER_HPP_ - -#include "UnscentedKalmanFilterBase.hpp" -#include "SquareRootFilterBase.hpp" - -namespace Kalman -{ - - /** - * @brief Square Root Unscented Kalman Filter (SR-UKF) - * - * @note This is the square-root implementation variant of UnscentedKalmanFilter - * - * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. - * Whenever "the paper" is referenced within this file then this paper is meant. - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class SquareRootUnscentedKalmanFilter : public UnscentedKalmanFilterBase, - public SquareRootFilterBase -{ -public: - //! Unscented Kalman Filter base type - typedef UnscentedKalmanFilterBase UnscentedBase; - - //! Square Root Filter base type - typedef SquareRootFilterBase SquareRootBase; - - //! Numeric Scalar Type inherited from base - using typename UnscentedBase::T; - - //! State Type inherited from base - using typename UnscentedBase::State; - - //! Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = typename UnscentedBase::template MeasurementModelType; - - //! System Model Type - template class CovarianceBase> - using SystemModelType = typename UnscentedBase::template SystemModelType; - -protected: - //! The number of sigma points (depending on state dimensionality) - using UnscentedBase::SigmaPointCount; - - //! Matrix type containing the sigma state or measurement points - template - using SigmaPoints = typename UnscentedBase::template SigmaPoints; - - //! Kalman Gain Matrix Type - template - using KalmanGain = Kalman::KalmanGain; - -protected: - // Member variables - - //! State Estimate - using UnscentedBase::x; - - //! Square Root of State Covariance - using SquareRootBase::S; - - //! Sigma points (state) - using UnscentedBase::sigmaStatePoints; - -public: - /** - * Constructor - * - * See paper for detailed parameter explanation - * - * @param alpha Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) - * @param beta Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) - * @param kappa Secondary scaling parameter (usually 0) - */ - SquareRootUnscentedKalmanFilter(T alpha = T(1), T beta = T(2), T kappa = T(0)) - : UnscentedKalmanFilterBase(alpha, beta, kappa) - { - // Init covariance to identity - S.setIdentity(); - } - - /** - * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) - * - * @param [in] s The System model - * @return The updated state estimate - */ - template class CovarianceBase> - const State & predict(const SystemModelType & s) - { - // predict state (without control) - Control u; - u.setZero(); - return predict(s, u); - } - - /** - * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model - * - * @param [in] s The System model - * @param [in] u The Control input vector - * @return The updated state estimate - */ - template class CovarianceBase> - const State & predict(const SystemModelType & s, const Control & u) - { - // Compute sigma points - computeSigmaPoints(); - - // Compute predicted state - x = this->template computeStatePrediction(s, u); - - // Compute predicted covariance - if(!computeCovarianceSquareRootFromSigmaPoints(x, sigmaStatePoints, s.getCovarianceSquareRoot(), - S)) - { - // TODO: handle numerical error - assert(false); - } - - // Return predicted state - return this->getState(); - } - - /** - * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model - * - * @param [in] m The Measurement model - * @param [in] z The measurement vector - * @return The updated state estimate - */ - template class CovarianceBase> - const State & update( - const MeasurementModelType & m, - const Measurement & z) - { - SigmaPoints sigmaMeasurementPoints; - - // Predict measurement (and corresponding sigma points) - Measurement y = this->template computeMeasurementPrediction(m, - sigmaMeasurementPoints); - - // Compute square root innovation covariance - CovarianceSquareRoot S_y; - if(!computeCovarianceSquareRootFromSigmaPoints(y, sigmaMeasurementPoints, - m.getCovarianceSquareRoot(), S_y)) - { - // TODO: handle numerical error - assert(false); - } - - KalmanGain K; - computeKalmanGain(y, sigmaMeasurementPoints, S_y, K); - - // Update state - x += K * ( z - y ); - - // Update state covariance - if(!updateStateCovariance(K, S_y)) { - // TODO: handle numerical error - assert(false); - } - - return this->getState(); - } - -protected: - /** - * @brief Compute sigma points from current state estimate and state covariance - * - * @note This covers equations (17) and (22) of Algorithm 3.1 in the Paper - */ - bool computeSigmaPoints() - { - // Get square root of covariance - Matrix _S = S.matrixL().toDenseMatrix(); - - // Set left "block" (first column) - sigmaStatePoints.template leftCols<1>() = x; - // Set center block with x + gamma * S - sigmaStatePoints.template block(0, 1) = - ( this->gamma * _S).colwise() + x; - // Set right block with x - gamma * S - sigmaStatePoints.template rightCols() = - (-this->gamma * _S).colwise() + x; - - return true; - } - - /** - * @brief Compute the Covariance Square root from sigma points and noise covariance - * - * @note This covers equations (20) and (21) as well as (25) and (26) of Algorithm 3.1 in the Paper - * - * @param [in] mean The mean predicted state or measurement - * @param [in] sigmaPoints the predicted sigma state or measurement points - * @param [in] noiseCov The system or measurement noise covariance (as square root) - * @param [out] cov The propagated state or innovation covariance (as square root) - * - * @return True on success, false if a numerical error is encountered when updating the matrix - */ - template - bool computeCovarianceSquareRootFromSigmaPoints( - const Type & mean, const SigmaPoints & sigmaPoints, - const CovarianceSquareRoot & noiseCov, CovarianceSquareRoot & cov) - { - // Compute QR decomposition of (transposed) augmented matrix - Matrix tmp; - tmp.template topRows<2 * State::RowsAtCompileTime>() = std::sqrt(this->sigmaWeights_c[1]) * - ( sigmaPoints.template rightCols().colwise() - mean).transpose(); - tmp.template bottomRows() = noiseCov.matrixU().toDenseMatrix(); - - // TODO: Use ColPivHouseholderQR - Eigen::HouseholderQR qr(tmp); - - // Set R matrix as upper triangular square root - cov.setU(qr.matrixQR().template topRightCorner()); - - // Perform additional rank 1 update - // TODO: According to the paper (Section 3, "Cholesky factor updating") the update - // is defined using the square root of the scalar, however the correct result - // is obtained when using the weight directly rather than using the square root - // It should be checked whether this is a bug in Eigen or in the Paper - // T nu = std::copysign( std::sqrt(std::abs(sigmaWeights_c[0])), sigmaWeights_c[0]); - T nu = this->sigmaWeights_c[0]; - cov.rankUpdate(sigmaPoints.template leftCols<1>() - mean, nu); - - return cov.info() == Eigen::Success; - } - - /** - * @brief Compute the Kalman Gain from predicted measurement and sigma points and the innovation covariance. - * - * @note This covers equations (27) and (28) of Algorithm 3.1 in the Paper - * - * We need to solve the equation \f$ K (S_y S_y^T) = P \f$ for \f$ K \f$ using backsubstitution. - * In order to apply standard backsubstitution using the `solve` method we must bring the - * equation into the form - * \f[ AX = B \qquad \text{with } A = S_yS_y^T \f] - * Thus we transpose the whole equation to obtain - * \f{align*}{ - * ( K (S_yS_y^T))^T &= P^T \Leftrightarrow \\ - * (S_yS_y^T)^T K^T &= P^T \Leftrightarrow \\ - * (S_yS_y^T) K^T &= P^T - *\f} - * Hence our \f$ X := K^T\f$ and \f$ B:= P^T \f$ - * - * @param [in] y The predicted measurement - * @param [in] sigmaMeasurementPoints The predicted sigma measurement points - * @param [in] S_y The innovation covariance as square-root - * @param [out] K The computed Kalman Gain matrix \f$ K \f$ - */ - template - bool computeKalmanGain( - const Measurement & y, - const SigmaPoints & sigmaMeasurementPoints, - const CovarianceSquareRoot & S_y, - KalmanGain & K) - { - // Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs - // when Measurement::RowsAtCompileTime == 1 AND State::RowsAtCompileTime >= 8 - decltype(sigmaStatePoints) W = this->sigmaWeights_c.transpose().template replicate(); - Matrix P = - (sigmaStatePoints.colwise() - x).cwiseProduct(W).eval() * - (sigmaMeasurementPoints.colwise() - y).transpose(); - - K = S_y.solve(P.transpose()).transpose(); - return true; - } - - /** - * @brief Update the state covariance matrix using the Kalman Gain and the Innovation Covariance - * - * @note This covers equations (29) and (30) of Algorithm 3.1 in the Paper - * - * @param [in] K The computed Kalman Gain matrix - * @param [in] S_y The innovation covariance as square-root - * @return True on success, false if a numerical error is encountered when updating the matrix - */ - template - bool updateStateCovariance( - const KalmanGain & K, - const CovarianceSquareRoot & S_y) - { - KalmanGain U = K * S_y.matrixL(); - for(int i = 0; i < U.cols(); ++i) { - S.rankUpdate(U.col(i), -1); - - if(S.info() == Eigen::NumericalIssue) { - // TODO: handle numerical issues in some sensible way - return false; - } - } - - return true; - } -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/StandardBase.hpp b/new_super_vision_filter/include/kalman/StandardBase.hpp deleted file mode 100644 index d3b88af9f..000000000 --- a/new_super_vision_filter/include/kalman/StandardBase.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_STANDARDBASE_HPP_ -#define KALMAN_STANDARDBASE_HPP_ - -#include "Types.hpp" - -namespace Kalman -{ - - /** - * @brief Abstract base class for standard (non-square root) filters and models - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class StandardBase -{ -protected: - //! Covariance - Covariance P; - -public: - /** - * Get covariance - */ - const Covariance & getCovariance() const - { - return P; - } - - /** - * Get covariance (as square root) - */ - CovarianceSquareRoot getCovarianceSquareRoot() const - { - return CovarianceSquareRoot(P); - } - - /** - * Set Covariance - */ - bool setCovariance(const Covariance & covariance) - { - P = covariance; - return true; - } - - /** - * @brief Set Covariance using Square Root - * - * @param covSquareRoot Lower triangular Matrix representing the covariance - * square root (i.e. P = LLˆT) - */ - bool setCovarianceSquareRoot(const Covariance & covSquareRoot) - { - CovarianceSquareRoot S; - S.setL(covSquareRoot); - P = S.reconstructedMatrix(); - return true; - } - -protected: - StandardBase() - { - P.setIdentity(); - } -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/StandardFilterBase.hpp b/new_super_vision_filter/include/kalman/StandardFilterBase.hpp deleted file mode 100644 index bf2147f40..000000000 --- a/new_super_vision_filter/include/kalman/StandardFilterBase.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_STANDARDFILTERBASE_HPP_ -#define KALMAN_STANDARDFILTERBASE_HPP_ - -#include "StandardBase.hpp" - -namespace Kalman -{ - - /** - * @brief Abstract base class for standard (non-square root) filters - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class StandardFilterBase : public StandardBase -{ -protected: - //! Standard Base Type - typedef StandardBase Base; - - //! Covariance matrix - using Base::P; -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/SystemModel.hpp b/new_super_vision_filter/include/kalman/SystemModel.hpp deleted file mode 100644 index 53a4e1ee8..000000000 --- a/new_super_vision_filter/include/kalman/SystemModel.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_SYSTEMMODEL_HPP_ -#define KALMAN_SYSTEMMODEL_HPP_ - -#include - -#include "Matrix.hpp" -#include "StandardBase.hpp" - -namespace Kalman -{ - /** - * @brief Abstract base class of all system models - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - * @param ControlType The vector-type of the control input (usually some type derived from Kalman::Vector) - * @param CovarianceBase The class template used for covariance storage (must be either StandardBase or SquareRootBase) - */ -template, - template class CovarianceBase = StandardBase> -class SystemModel : public CovarianceBase -{ - static_assert(/*StateType::RowsAtCompileTime == Dynamic ||*/ StateType::RowsAtCompileTime > 0, - "State vector must contain at least 1 element" /* or be dynamic */); - static_assert(/*ControlType::RowsAtCompileTime == Dynamic ||*/ ControlType::RowsAtCompileTime >= - 0, - "Control vector must contain at least 0 elements" /* or be dynamic */); - static_assert(std::is_same::value, - "State and Control scalar types must be identical"); - -public: - //! System state type - typedef StateType State; - - //! System control input type - typedef ControlType Control; - -public: - /** - * @brief State Transition Function f - * - * Computes the predicted system state in the next timestep given - * the current state x and the control input u - */ - virtual State f(const State & x, const Control & u) const = 0; - -protected: - SystemModel() {} - virtual ~SystemModel() {} -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/Types.hpp b/new_super_vision_filter/include/kalman/Types.hpp deleted file mode 100644 index 0af0cfc3b..000000000 --- a/new_super_vision_filter/include/kalman/Types.hpp +++ /dev/null @@ -1,75 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_TYPES_HPP_ -#define KALMAN_TYPES_HPP_ - -#include "Matrix.hpp" - -namespace Kalman -{ - /** - * @class Kalman::SquareMatrix - * @brief Template type representing a square matrix - * @param T The numeric scalar type - * @param N The dimensionality of the Matrix - */ -template -using SquareMatrix = Matrix; - - /** - * @class Kalman::Covariance - * @brief Template type for covariance matrices - * @param Type The vector type for which to generate a covariance (usually a state or measurement type) - */ -template -using Covariance = SquareMatrix; - - /** - * @class Kalman::CovarianceSquareRoot - * @brief Template type for covariance square roots - * @param Type The vector type for which to generate a covariance (usually a state or measurement type) - */ -template -using CovarianceSquareRoot = Cholesky>; - - /** - * @class Kalman::KalmanGain - * @brief Template type of Kalman Gain - * @param State The system state type - * @param Measurement The measurement type - */ -template -using KalmanGain = Matrix; - - /** - * @class Kalman::Jacobian - * @brief Template type of jacobian of A w.r.t. B - */ -template -using Jacobian = Matrix; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp b/new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp deleted file mode 100644 index eaffc0084..000000000 --- a/new_super_vision_filter/include/kalman/UnscentedKalmanFilter.hpp +++ /dev/null @@ -1,282 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_UNSCENTEDKALMANFILTER_HPP_ -#define KALMAN_UNSCENTEDKALMANFILTER_HPP_ - -#include "UnscentedKalmanFilterBase.hpp" -#include "StandardFilterBase.hpp" - -namespace Kalman -{ - - /** - * @brief Unscented Kalman Filter (UKF) - * - * @note It is recommended to use the square-root implementation SquareRootUnscentedKalmanFilter of this filter - * - * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. - * Whenever "the paper" is referenced within this file then this paper is meant. - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class UnscentedKalmanFilter : public UnscentedKalmanFilterBase, - public StandardFilterBase -{ -public: - //! Unscented Kalman Filter base type - typedef UnscentedKalmanFilterBase UnscentedBase; - - //! Standard Filter base type - typedef StandardFilterBase StandardBase; - - //! Numeric Scalar Type inherited from base - using typename UnscentedBase::T; - - //! State Type inherited from base - using typename UnscentedBase::State; - - //! Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = typename UnscentedBase::template MeasurementModelType; - - //! System Model Type - template class CovarianceBase> - using SystemModelType = typename UnscentedBase::template SystemModelType; - -protected: - //! The number of sigma points (depending on state dimensionality) - using UnscentedBase::SigmaPointCount; - - //! Matrix type containing the sigma state or measurement points - template - using SigmaPoints = typename UnscentedBase::template SigmaPoints; - - //! Kalman Gain Matrix Type - template - using KalmanGain = Kalman::KalmanGain; - -protected: - // Member variables - - //! State Estimate - using UnscentedBase::x; - - //! State Covariance - using StandardBase::P; - - //! Sigma points (state) - using UnscentedBase::sigmaStatePoints; - -public: - /** - * Constructor - * - * See paper for detailed parameter explanation - * - * @param alpha Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) - * @param beta Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) - * @param kappa Secondary scaling parameter (usually 0) - */ - UnscentedKalmanFilter(T alpha = T(1), T beta = T(2), T kappa = T(0)) - : UnscentedKalmanFilterBase(alpha, beta, kappa) - { - // Init covariance to identity - P.setIdentity(); - } - - /** - * @brief Perform filter prediction step using system model and no control input (i.e. \f$ u = 0 \f$) - * - * @param [in] s The System model - * @return The updated state estimate - */ - template class CovarianceBase> - const State & predict(const SystemModelType & s) - { - // predict state (without control) - Control u; - u.setZero(); - return predict(s, u); - } - - /** - * @brief Perform filter prediction step using control input \f$u\f$ and corresponding system model - * - * @param [in] s The System model - * @param [in] u The Control input vector - * @return The updated state estimate - */ - template class CovarianceBase> - const State & predict(const SystemModelType & s, const Control & u) - { - // Compute sigma points - if(!computeSigmaPoints()) { - // TODO: handle numerical error - assert(false); - } - - // Compute predicted state - x = this->template computeStatePrediction(s, u); - - // Compute predicted covariance - computeCovarianceFromSigmaPoints(x, sigmaStatePoints, s.getCovariance(), P); - - // Return predicted state - return this->getState(); - } - - /** - * @brief Perform filter update step using measurement \f$z\f$ and corresponding measurement model - * - * @param [in] m The Measurement model - * @param [in] z The measurement vector - * @return The updated state estimate - */ - template class CovarianceBase> - const State & update( - const MeasurementModelType & m, - const Measurement & z) - { - SigmaPoints sigmaMeasurementPoints; - - // Predict measurement (and corresponding sigma points) - Measurement y = this->template computeMeasurementPrediction(m, - sigmaMeasurementPoints); - - // Compute innovation covariance - Covariance P_yy; - computeCovarianceFromSigmaPoints(y, sigmaMeasurementPoints, m.getCovariance(), P_yy); - - KalmanGain K; - computeKalmanGain(y, sigmaMeasurementPoints, P_yy, K); - - // Update state - x += K * ( z - y ); - - // Update state covariance - updateStateCovariance(K, P_yy); - - return this->getState(); - } - -protected: - /** - * @brief Compute sigma points from current state estimate and state covariance - * - * @note This covers equations (6) and (9) of Algorithm 2.1 in the Paper - */ - bool computeSigmaPoints() - { - // Get square root of covariance - CovarianceSquareRoot llt; - llt.compute(P); - if(llt.info() != Eigen::Success) { - return false; - } - - SquareMatrix _S = llt.matrixL().toDenseMatrix(); - - // Set left "block" (first column) - sigmaStatePoints.template leftCols<1>() = x; - // Set center block with x + gamma * S - sigmaStatePoints.template block(0, 1) = - ( this->gamma * _S).colwise() + x; - // Set right block with x - gamma * S - sigmaStatePoints.template rightCols() = - (-this->gamma * _S).colwise() + x; - - return true; - } - - /** - * @brief Compute the Covariance from sigma points and noise covariance - * - * @param [in] mean The mean predicted state or measurement - * @param [in] sigmaPoints the predicted sigma state or measurement points - * @param [in] noiseCov The system or measurement noise covariance - * @param [out] cov The propagated state or innovation covariance - * - * @return True on success, false if a numerical error is encountered when updating the matrix - */ - template - bool computeCovarianceFromSigmaPoints( - const Type & mean, const SigmaPoints & sigmaPoints, - const Covariance & noiseCov, Covariance & cov) - { - decltype(sigmaPoints) W = this->sigmaWeights_c.transpose().template replicate(); - decltype(sigmaPoints) tmp = (sigmaPoints.colwise() - mean); - cov = tmp.cwiseProduct(W) * tmp.transpose() + noiseCov; - - return true; - } - - /** - * @brief Compute the Kalman Gain from predicted measurement and sigma points and the innovation covariance. - * - * @note This covers equations (11) and (12) of Algorithm 2.1 in the Paper - * - * @param [in] y The predicted measurement - * @param [in] sigmaMeasurementPoints The predicted sigma measurement points - * @param [in] P_yy The innovation covariance - * @param [out] K The computed Kalman Gain matrix \f$ K \f$ - */ - template - bool computeKalmanGain( - const Measurement & y, - const SigmaPoints & sigmaMeasurementPoints, - const Covariance & P_yy, - KalmanGain & K) - { - // Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs - // when Measurement::RowsAtCompileTime == 1 AND State::RowsAtCompileTime >= 8 - decltype(sigmaStatePoints) W = this->sigmaWeights_c.transpose().template replicate(); - Matrix P_xy = - (sigmaStatePoints.colwise() - x).cwiseProduct(W).eval() * - (sigmaMeasurementPoints.colwise() - y).transpose(); - - K = P_xy * P_yy.inverse(); - return true; - } - - /** - * @brief Update the state covariance matrix using the Kalman Gain and the Innovation Covariance - * - * @note This covers equation (14) of Algorithm 2.1 in the Paper - * - * @param [in] K The computed Kalman Gain matrix - * @param [in] P_yy The innovation covariance - * @return True on success, false if a numerical error is encountered when updating the matrix - */ - template - bool updateStateCovariance( - const KalmanGain & K, - const Covariance & P_yy) - { - P -= K * P_yy * K.transpose(); - return true; - } -}; -} - -#endif diff --git a/new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp b/new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp deleted file mode 100644 index 63795f610..000000000 --- a/new_super_vision_filter/include/kalman/UnscentedKalmanFilterBase.hpp +++ /dev/null @@ -1,239 +0,0 @@ -// The MIT License (MIT) -// -// Copyright (c) 2015 Markus Herb -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -// THE SOFTWARE. -#ifndef KALMAN_UNSCENTEDKALMANFILTERBASE_HPP_ -#define KALMAN_UNSCENTEDKALMANFILTERBASE_HPP_ - -#include - -#include "KalmanFilterBase.hpp" -#include "SystemModel.hpp" -#include "MeasurementModel.hpp" - -namespace Kalman -{ - - /** - * @brief Abstract Base for Unscented Kalman Filters - * - * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. - * Whenever "the paper" is referenced within this file then this paper is meant. - * - * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) - */ -template -class UnscentedKalmanFilterBase : public KalmanFilterBase -{ -public: - // Public typedefs - //! Kalman Filter base type - typedef KalmanFilterBase Base; - - //! Numeric Scalar Type inherited from base - using typename Base::T; - - //! State Type inherited from base - using typename Base::State; - - //! Measurement Model Type - template class CovarianceBase> - using MeasurementModelType = MeasurementModel; - - //! System Model Type - template class CovarianceBase> - using SystemModelType = SystemModel; - -protected: - // Protected typedefs - //! The number of sigma points (depending on state dimensionality) - static constexpr int SigmaPointCount = 2 * State::RowsAtCompileTime + 1; - - //! Vector containg the sigma scaling weights - typedef Vector SigmaWeights; - - //! Matrix type containing the sigma state or measurement points - template - using SigmaPoints = Matrix; - -protected: - // Member variables - - //! State Estimate - using Base::x; - - //! Sigma weights (m) - SigmaWeights sigmaWeights_m; - //! Sigma weights (c) - SigmaWeights sigmaWeights_c; - - //! Sigma points (state) - SigmaPoints sigmaStatePoints; - - // Weight parameters - T alpha; //!< Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) - T beta; //!< Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) - T kappa; //!< Secondary scaling parameter (usually 0) - T gamma; //!< \f$ \gamma = \sqrt{L + \lambda} \f$ with \f$ L \f$ being the state dimensionality - T lambda; //!< \f$ \lambda = \alpha^2 ( L + \kappa ) - L\f$ with \f$ L \f$ being the state dimensionality - -protected: - /** - * Constructor - * - * See paper for parameter explanation - * - * @param _alpha Scaling parameter for spread of sigma points (usually 1e-4 <= alpha <= 1) - * @param _beta Parameter for prior knowledge about the distribution (beta = 2 is optimal for Gaussian) - * @param _kappa Secondary scaling parameter (usually 0) - */ - UnscentedKalmanFilterBase(T _alpha = T(1), T _beta = T(2), T _kappa = T(0)) - : alpha(_alpha), beta(_beta), kappa(_kappa) - { - // Pre-compute all weights - computeWeights(); - - // Setup state and covariance - x.setZero(); - } - - /** - * @brief Compute predicted state using system model and control input - * - * @param [in] s The System Model - * @param [in] u The Control input - * @return The predicted state - */ - template class CovarianceBase> - State computeStatePrediction( - const SystemModelType & s, - const Control & u) - { - // Pass each sigma point through non-linear state transition function - computeSigmaPointTransition(s, u); - - // Compute predicted state from predicted sigma points - return computePredictionFromSigmaPoints(sigmaStatePoints); - } - - /** - * @brief Compute predicted measurement using measurement model and predicted sigma measurements - * - * @param [in] m The Measurement Model - * @param [in] sigmaMeasurementPoints The predicted sigma measurement points - * @return The predicted measurement - */ - template class CovarianceBase> - Measurement computeMeasurementPrediction( - const MeasurementModelType & m, SigmaPoints & sigmaMeasurementPoints) - { - // Predict measurements for each sigma point - computeSigmaPointMeasurements(m, sigmaMeasurementPoints); - - // Predict measurement from sigma measurement points - return computePredictionFromSigmaPoints(sigmaMeasurementPoints); - } - - /** - * @brief Compute sigma weights - */ - void computeWeights() - { - T L = T(State::RowsAtCompileTime); - lambda = alpha * alpha * ( L + kappa ) - L; - gamma = std::sqrt(L + lambda); - - // Make sure L != -lambda to avoid division by zero - assert(std::abs(L + lambda) > 1e-6); - - // Make sure L != -kappa to avoid division by zero - assert(std::abs(L + kappa) > 1e-6); - - T W_m_0 = lambda / ( L + lambda ); - T W_c_0 = W_m_0 + (T(1) - alpha * alpha + beta); - T W_i = T(1) / ( T(2) * alpha * alpha * (L + kappa) ); - - // Make sure W_i > 0 to avoid square-root of negative number - assert(W_i > T(0) ); - - sigmaWeights_m[0] = W_m_0; - sigmaWeights_c[0] = W_c_0; - - for(int i = 1; i < SigmaPointCount; ++i) { - sigmaWeights_m[i] = W_i; - sigmaWeights_c[i] = W_i; - } - } - - /** - * @brief Predict expected sigma states from current sigma states using system model and control input - * - * @note This covers equation (18) of Algorithm 3.1 in the Paper - * - * @param [in] s The System Model - * @param [in] u The Control input - */ - template class CovarianceBase> - void computeSigmaPointTransition( - const SystemModelType & s, - const Control & u) - { - for( int i = 0; i < SigmaPointCount; ++i ) { - sigmaStatePoints.col(i) = s.f(sigmaStatePoints.col(i), u); - } - } - - /** - * @brief Predict the expected sigma measurements from predicted sigma states using measurement model - * - * @note This covers equation (23) of Algorithm 3.1 in the Paper - * - * @param [in] m The Measurement model - * @param [out] sigmaMeasurementPoints The struct of expected sigma measurements to be computed - */ - template class CovarianceBase> - void computeSigmaPointMeasurements( - const MeasurementModelType & m, - SigmaPoints & sigmaMeasurementPoints) - { - for( int i = 0; i < SigmaPointCount; ++i ) { - sigmaMeasurementPoints.col(i) = m.h(sigmaStatePoints.col(i) ); - } - } - - /** - * @brief Compute state or measurement prediciton from sigma points using pre-computed sigma weights - * - * @note This covers equations (19) and (24) of Algorithm 3.1 in the Paper - * - * @param [in] sigmaPoints The computed sigma points of the desired type (state or measurement) - * @return The prediction - */ - template - Type computePredictionFromSigmaPoints(const SigmaPoints & sigmaPoints) - { - // Use efficient matrix x vector computation - return sigmaPoints * sigmaWeights_m; - } -}; -} - -#endif diff --git a/new_super_vision_filter/kalman b/new_super_vision_filter/kalman new file mode 160000 index 000000000..9f40c2f7b --- /dev/null +++ b/new_super_vision_filter/kalman @@ -0,0 +1 @@ +Subproject commit 9f40c2f7bedb08e964297a00a2b9360036590896 From 7cf7909c976b4cce2b060bb8ea9e57390499b3fd Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 3 Mar 2026 22:36:42 -0500 Subject: [PATCH 31/35] Add new vision to autonomy stack --- ateam_bringup/launch/autonomy.launch.xml | 2 +- .../launch/autonomy_new_vision.launch.xml | 28 ----- .../launch/new_vision_bringup.launch.py | 104 ------------------ 3 files changed, 1 insertion(+), 133 deletions(-) delete mode 100644 ateam_bringup/launch/autonomy_new_vision.launch.xml delete mode 100644 ateam_bringup/launch/new_vision_bringup.launch.py diff --git a/ateam_bringup/launch/autonomy.launch.xml b/ateam_bringup/launch/autonomy.launch.xml index 6f0a5f00f..6423da7e6 100644 --- a/ateam_bringup/launch/autonomy.launch.xml +++ b/ateam_bringup/launch/autonomy.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/ateam_bringup/launch/autonomy_new_vision.launch.xml b/ateam_bringup/launch/autonomy_new_vision.launch.xml deleted file mode 100644 index 6423da7e6..000000000 --- a/ateam_bringup/launch/autonomy_new_vision.launch.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ateam_bringup/launch/new_vision_bringup.launch.py b/ateam_bringup/launch/new_vision_bringup.launch.py deleted file mode 100644 index c0e67048b..000000000 --- a/ateam_bringup/launch/new_vision_bringup.launch.py +++ /dev/null @@ -1,104 +0,0 @@ -# Copyright 2021 A Team -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -from ateam_bringup.substitutions import PackageLaunchFileSubstitution -import launch -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.launch_description_sources import FrontendLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - return launch.LaunchDescription([ - DeclareLaunchArgument('start_sim', default_value='True'), - DeclareLaunchArgument('start_gc', default_value='True'), - DeclareLaunchArgument('start_ui', default_value='True'), - DeclareLaunchArgument('headless_sim', default_value='True'), - DeclareLaunchArgument('sim_radio_ip', default_value='127.0.0.1'), - DeclareLaunchArgument('team_name', default_value='A-Team'), - - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_bringup', - 'ssl_erforce_sim.launch.xml')), - launch_arguments={ - 'headless': LaunchConfiguration('headless_sim') - }.items(), - condition=IfCondition(LaunchConfiguration('start_sim')) - ), - - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_bringup', - 'ssl_game_controller.launch.xml')), - condition=IfCondition(LaunchConfiguration('start_gc')) - ), - - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_bringup', - 'league_bridges.launch.xml')), - launch_arguments={ - 'gc_net_interface_address': '172.17.0.1', - 'gc_ip_address': '172.17.0.2', - 'vision_net_interface_address': '127.0.0.1', - 'vision_port': '10020', - 'team_name': LaunchConfiguration('team_name') - }.items() - ), - - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_bringup', - 'autonomy.launch.xml')), - launch_arguments={ - 'team_name': LaunchConfiguration('team_name'), - 'use_emulated_ballsense': 'False', - 'vision_offset_robot_x': '0.0', - 'vision_offset_robot_y': '0.0', - }.items() - ), - - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_bringup', - 'ui.launch.xml')), - condition=IfCondition(LaunchConfiguration('start_ui')) - ), - - Node( - package='ateam_ssl_simulation_radio_bridge', - executable='ssl_simulation_radio_bridge_node', - name='radio_bridge', - parameters=[{ - 'ssl_sim_radio_ip': LaunchConfiguration('sim_radio_ip'), - 'gc_team_name': LaunchConfiguration('team_name') - }] - ), - - IncludeLaunchDescription( - FrontendLaunchDescriptionSource( - PackageLaunchFileSubstitution('ateam_joystick_control', - 'joystick_controller.launch.xml') - ) - ) - ]) From fea56b76b1aa6281268bb9af6331dfdbe9c38906 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 3 Mar 2026 22:40:53 -0500 Subject: [PATCH 32/35] Remove old comment --- new_super_vision_filter/src/vision_filter_node.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index ade78c415..4126e5ff6 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -40,13 +40,6 @@ namespace new_super_vision class VisionFilterNode : public rclcpp::Node { - // Important things this needs to do, which could be broken out into objects that this holds - // and uses when the node is ticked... - - // Check quality of stuff overall - // Keep track of individual cameras - can be 1 - many - // Fuse the info from those cameras into tracks - // Output final prediction for robots and ball(s) public: explicit VisionFilterNode(const rclcpp::NodeOptions & options) From 3b32cdde242c59b49d99fb7aa37037fae6e6f696 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 3 Mar 2026 22:42:10 -0500 Subject: [PATCH 33/35] Update package description --- new_super_vision_filter/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/new_super_vision_filter/package.xml b/new_super_vision_filter/package.xml index 82cdef06f..180ede5a7 100644 --- a/new_super_vision_filter/package.xml +++ b/new_super_vision_filter/package.xml @@ -2,8 +2,8 @@ new_vision_filter - 0.0.0 - TODO: Package description + 1.0.0 + wahoo! we can filter vision data! christian Apache-2.0 From 4f2831c174b8c091d54d7f88cf9053e8a39f0c30 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 15 Mar 2026 22:03:59 -0400 Subject: [PATCH 34/35] Change track to measurement, reenable linting --- new_super_vision_filter/CMakeLists.txt | 13 ++--- new_super_vision_filter/package.xml | 2 +- new_super_vision_filter/src/filtered_ball.cpp | 12 ++--- new_super_vision_filter/src/filtered_ball.hpp | 6 +-- .../src/filtered_robot.cpp | 20 +++---- .../src/filtered_robot.hpp | 6 +-- .../{ball_track.hpp => ball_measurement.hpp} | 10 ++-- ...{robot_track.hpp => robot_measurement.hpp} | 11 ++-- .../src/vision_filter_node.cpp | 52 +++++++++---------- new_super_vision_filter/test/filter_test.cpp | 14 ++--- 10 files changed, 70 insertions(+), 76 deletions(-) rename new_super_vision_filter/src/measurements/{ball_track.hpp => ball_measurement.hpp} (87%) rename new_super_vision_filter/src/measurements/{robot_track.hpp => robot_measurement.hpp} (91%) diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt index a95ad9db3..5a92fa87f 100644 --- a/new_super_vision_filter/CMakeLists.txt +++ b/new_super_vision_filter/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.8) project(new_vision_filter) -# if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") -# add_compile_options(-Wall -Wextra -Wpedantic) -# endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() # find dependencies find_package(ament_cmake REQUIRED) @@ -53,13 +53,6 @@ install(TARGETS ${PROJECT_NAME} if(BUILD_TESTING) find_package(ament_cmake_lint_cmake REQUIRED) find_package(ament_cmake_gtest REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) file(GLOB_RECURSE AMENT_LINT_AUTO_FILE_EXCLUDE "kalman/*" "kalman/cmake/*" diff --git a/new_super_vision_filter/package.xml b/new_super_vision_filter/package.xml index 180ede5a7..51d14b445 100644 --- a/new_super_vision_filter/package.xml +++ b/new_super_vision_filter/package.xml @@ -1,7 +1,7 @@ - new_vision_filter + new_super_vision_filter 1.0.0 wahoo! we can filter vision data! christian diff --git a/new_super_vision_filter/src/filtered_ball.cpp b/new_super_vision_filter/src/filtered_ball.cpp index f6b7b0238..1c60efa1f 100644 --- a/new_super_vision_filter/src/filtered_ball.cpp +++ b/new_super_vision_filter/src/filtered_ball.cpp @@ -1,11 +1,11 @@ #include "filtered_ball.hpp" -FilteredBall::FilteredBall(const BallTrack & track) +FilteredBall::FilteredBall(const BallMeasurement & measurement) { PosState initial_state_xy; initial_state_xy << - track.pos.x(), - track.pos.y(), + measurement.pos.x(), + measurement.pos.y(), 0, 0; posFilterXY.init(initial_state_xy); @@ -20,7 +20,7 @@ FilteredBall::FilteredBall(const BallTrack & track) posFilterXY.setCovariance(xy_covariance); } -void FilteredBall::update(const BallTrack & track) +void FilteredBall::update(const BallMeasurement & measurement) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) @@ -35,7 +35,7 @@ void FilteredBall::update(const BallTrack & track) const std::chrono::time_point now = std::chrono::steady_clock::now(); // If it's been too long, don't use this message - if (now - track.timestamp > update_threshold || is_new) { + if (now - measurement.timestamp > update_threshold || is_new) { return; } // Predict state forward @@ -47,7 +47,7 @@ void FilteredBall::update(const BallTrack & track) // Update state estimate (returned) // Update covariance estimate (contained in filter) // All encompassed by the .update() function - posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); + posXYEstimate = posFilterXY.update(measurementModelXY, measurement.pos); } ateam_msgs::msg::VisionStateBall FilteredBall::toMsg() diff --git a/new_super_vision_filter/src/filtered_ball.hpp b/new_super_vision_filter/src/filtered_ball.hpp index df5bf2b96..11d03120d 100644 --- a/new_super_vision_filter/src/filtered_ball.hpp +++ b/new_super_vision_filter/src/filtered_ball.hpp @@ -27,7 +27,7 @@ #include #include "kalman/ExtendedKalmanFilter.hpp" #include "filter_types.hpp" -#include "measurements/ball_track.hpp" +#include "measurements/ball_measurement.hpp" enum KickState { @@ -38,9 +38,9 @@ enum KickState class FilteredBall { public: - FilteredBall(const BallTrack & track); + FilteredBall(const BallMeasurement & measurement); - void update(const BallTrack & track); + void update(const BallMeasurement & measurement); ateam_msgs::msg::VisionStateBall toMsg(); diff --git a/new_super_vision_filter/src/filtered_robot.cpp b/new_super_vision_filter/src/filtered_robot.cpp index 0d0cd9dea..294bc71aa 100644 --- a/new_super_vision_filter/src/filtered_robot.cpp +++ b/new_super_vision_filter/src/filtered_robot.cpp @@ -20,7 +20,7 @@ #include "filtered_robot.hpp" #include "filter_types.hpp" -#include "measurements/robot_track.hpp" +#include "measurements/robot_measurement.hpp" #include #include @@ -31,14 +31,14 @@ // or https://thekalmanfilter.com/kalman-filter-explained-simply/ // OR https://github.com/mherb/kalman/blob/master/examples/Robot1/main.cpp -FilteredRobot::FilteredRobot(const RobotTrack & track, ateam_common::TeamColor & team_color) -: posFilterXY(), posFilterW(), bot_id(track.getId()), team(team_color) +FilteredRobot::FilteredRobot(const RobotMeasurement & measurement, ateam_common::TeamColor & team_color) +: posFilterXY(), posFilterW(), bot_id(measurement.getId()), team(team_color) { // Initialize XY KF PosState initial_state_xy; initial_state_xy << - track.pos.x(), - track.pos.y(), + measurement.pos.x(), + measurement.pos.y(), 0, 0; posFilterXY.init(initial_state_xy); @@ -60,7 +60,7 @@ FilteredRobot::FilteredRobot(const RobotTrack & track, ateam_common::TeamColor & */ AngleState initial_state_w; initial_state_w << - track.angle.w(), + measurement.angle.w(), 0; posFilterW.init(initial_state_w); /* @@ -73,7 +73,7 @@ FilteredRobot::FilteredRobot(const RobotTrack & track, ateam_common::TeamColor & posFilterW.setCovariance(w_covariance); } -void FilteredRobot::update(const RobotTrack & track) +void FilteredRobot::update(const RobotMeasurement & measurement) { // Make sure this detection isn't crazy off from our previous ones // (unless our filter is still new/only has a few measurements) @@ -88,7 +88,7 @@ void FilteredRobot::update(const RobotTrack & track) const std::chrono::time_point now = std::chrono::steady_clock::now(); // If it's been too long, don't use this message - if (now - track.getTimestamp() > update_threshold || is_new) { + if (now - measurement.getTimestamp() > update_threshold || is_new) { return; } // Predict state forward @@ -101,8 +101,8 @@ void FilteredRobot::update(const RobotTrack & track) // Update state estimate (returned) // Update covariance estimate (contained in filter) // All encompassed by the .update() function - posXYEstimate = posFilterXY.update(measurementModelXY, track.pos); - posWEstimate = posFilterW.update(measurementModelW, track.angle); + posXYEstimate = posFilterXY.update(measurementModelXY, measurement.pos); + posWEstimate = posFilterW.update(measurementModelW, measurement.angle); } ateam_msgs::msg::VisionStateRobot FilteredRobot::toMsg() diff --git a/new_super_vision_filter/src/filtered_robot.hpp b/new_super_vision_filter/src/filtered_robot.hpp index 848fe5a7d..b1e15bc91 100644 --- a/new_super_vision_filter/src/filtered_robot.hpp +++ b/new_super_vision_filter/src/filtered_robot.hpp @@ -22,7 +22,7 @@ #include "kalman/ExtendedKalmanFilter.hpp" #include "filter_types.hpp" -#include "measurements/robot_track.hpp" +#include "measurements/robot_measurement.hpp" #include #include @@ -32,9 +32,9 @@ class FilteredRobot { public: - FilteredRobot(const RobotTrack & track, ateam_common::TeamColor & team_color); + FilteredRobot(const RobotMeasurement & measurement, ateam_common::TeamColor & team_color); - void update(const RobotTrack & track); + void update(const RobotMeasurement & measurement); ateam_msgs::msg::VisionStateRobot toMsg(); diff --git a/new_super_vision_filter/src/measurements/ball_track.hpp b/new_super_vision_filter/src/measurements/ball_measurement.hpp similarity index 87% rename from new_super_vision_filter/src/measurements/ball_track.hpp rename to new_super_vision_filter/src/measurements/ball_measurement.hpp index 94dccb91e..552a30748 100644 --- a/new_super_vision_filter/src/measurements/ball_track.hpp +++ b/new_super_vision_filter/src/measurements/ball_measurement.hpp @@ -18,16 +18,16 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#ifndef BALL_TRACK_HPP_ -#define BALL_TRACK_HPP_ +#ifndef BALL_MEASUREMENT_HPP_ +#define BALL_MEASUREMENT_HPP_ #include "filter_types.hpp" #include -class BallTrack { +class BallMeasurement { public: - BallTrack(const ssl_league_msgs::msg::VisionDetectionBall & ball_detection, int & camera_id) + BallMeasurement(const ssl_league_msgs::msg::VisionDetectionBall & ball_detection, int & camera_id) : camera_id(camera_id) { PosMeasurement pos; @@ -41,4 +41,4 @@ class BallTrack { int camera_id; }; -#endif // BALL_TRACK_HPP_ +#endif // BALL_MEASUREMENT_HPP_ diff --git a/new_super_vision_filter/src/measurements/robot_track.hpp b/new_super_vision_filter/src/measurements/robot_measurement.hpp similarity index 91% rename from new_super_vision_filter/src/measurements/robot_track.hpp rename to new_super_vision_filter/src/measurements/robot_measurement.hpp index 3527775e1..a3b2cc95b 100644 --- a/new_super_vision_filter/src/measurements/robot_track.hpp +++ b/new_super_vision_filter/src/measurements/robot_measurement.hpp @@ -18,8 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#ifndef ROBOT_TRACK_HPP_ -#define ROBOT_TRACK_HPP_ +#ifndef ROBOT_MEASUREMENT_HPP_ +#define ROBOT_MEASUREMENT_HPP_ #include #include @@ -30,9 +30,9 @@ * @brief Robot position and angle measurement from a single camera detection, * used to provide updates to the Kalman filter. */ -class RobotTrack { +class RobotMeasurement { public: - RobotTrack( + RobotMeasurement( const ssl_league_msgs::msg::VisionDetectionRobot & bot_detection, int & camera_id, ateam_common::TeamColor & team @@ -44,6 +44,7 @@ class RobotTrack { bot_detection.pose.position.y; angle << bot_detection.pose.orientation.w; robot_id = bot_detection.robot_id; + timestamp = std::chrono::steady_clock::now(); } int getId() const @@ -67,4 +68,4 @@ class RobotTrack { }; -#endif // ROBOT_TRACK_HPP_ +#endif // ROBOT_MEASUREMENT_HPP_ diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 4126e5ff6..3510df95f 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -20,8 +20,8 @@ #include "camera.hpp" #include "filtered_robot.hpp" #include "filtered_ball.hpp" -#include "measurements/ball_track.hpp" -#include "measurements/robot_track.hpp" +#include "measurements/ball_measurement.hpp" +#include "measurements/robot_measurement.hpp" #include #include @@ -102,9 +102,9 @@ class VisionFilterNode : public rclcpp::Node std::vector yellow_robots; std::optional ball; - std::vector ball_tracks; - std::vector blue_tracks; - std::vector yellow_tracks; + std::vector ball_measurements; + std::vector blue_measurements; + std::vector yellow_measurements; // All this stuff interacts with other nodes (pub/sub related) std::array::SharedPtr, @@ -130,68 +130,68 @@ class VisionFilterNode : public rclcpp::Node cameras.try_emplace(detect_camera, detect_camera); } - // Create a track from each robot in this message + // Create a measurement from each robot in this message for (const auto & bot : detection.robots_yellow) { ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow; - yellow_tracks.push_back( - RobotTrack(bot, detect_camera, team_color) + yellow_measurements.push_back( + RobotMeasurement(bot, detect_camera, team_color) ); } for (const auto & bot : detection.robots_blue) { ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; - blue_tracks.push_back( - RobotTrack(bot, detect_camera, team_color) + blue_measurements.push_back( + RobotMeasurement(bot, detect_camera, team_color) ); } - // Create a track from each ball in this message + // Create a measurement from each ball in this message for (const auto & ball: detection.balls) { - ball_tracks.push_back( - BallTrack(ball, detect_camera) + ball_measurements.push_back( + BallMeasurement(ball, detect_camera) ); } } - // Sort our tracks to make sure they are in order of the time received + // Sort our measurements to make sure they are in order of the time received // in case created/processed them out of order - // Process all the new updates from the tracks - for (const auto & bot_track : blue_tracks) { + // Process all the new updates from the measurements + for (const auto & bot_measurement : blue_measurements) { ateam_common::TeamColor team_color = ateam_common::TeamColor::Blue; auto it = std::find_if( blue_robots.begin(), blue_robots.end(), - [bot_track](const FilteredRobot & bot){return bot_track.getId() == bot.getId();} + [bot_measurement](const FilteredRobot & bot){return bot_measurement.getId() == bot.getId();} ); if (it != blue_robots.end()) { blue_robots.push_back( - FilteredRobot(bot_track, team_color) + FilteredRobot(bot_measurement, team_color) ); } else { - it->update(bot_track); + it->update(bot_measurement); } } - for (const auto & bot_track : yellow_tracks) { + for (const auto & bot_measurement : yellow_measurements) { ateam_common::TeamColor team_color = ateam_common::TeamColor::Yellow; auto it = std::find_if( yellow_robots.begin(), yellow_robots.end(), - [bot_track](const FilteredRobot & bot){return bot_track.getId() == bot.getId();} + [bot_measurement](const FilteredRobot & bot){return bot_measurement.getId() == bot.getId();} ); if (it != yellow_robots.end()) { yellow_robots.push_back( - FilteredRobot(bot_track, team_color) + FilteredRobot(bot_measurement, team_color) ); } else { - it->update(bot_track); + it->update(bot_measurement); } } - for (const auto & ball_track : ball_tracks) { + for (const auto & ball_measurement : ball_measurements) { if (!ball.has_value()) { - ball = FilteredBall(ball_track); + ball = FilteredBall(ball_measurement); } else { - ball->update(ball_track); + ball->update(ball_measurement); } } } diff --git a/new_super_vision_filter/test/filter_test.cpp b/new_super_vision_filter/test/filter_test.cpp index 28838c854..d3b639470 100644 --- a/new_super_vision_filter/test/filter_test.cpp +++ b/new_super_vision_filter/test/filter_test.cpp @@ -30,7 +30,7 @@ #include #include "filtered_robot.hpp" -#include "measurements/robot_track.hpp" +#include "measurements/robot_measurement.hpp" namespace ateam_msgs { @@ -57,8 +57,8 @@ class FilteredRobotTest : public ::testing::Test void SetUp() override { int camera = 0; - auto track = RobotTrack(robot_msg, camera, team); - bot = std::make_unique(track, team); + auto measurement = RobotMeasurement(robot_msg, camera, team); + bot = std::make_unique(measurement, team); } }; @@ -73,16 +73,16 @@ TEST_F(FilteredRobotTest, WaitUntilOldEnough) fake_vision_data.pose.position.x = 1; fake_vision_data.pose.position.y = 1; if (i < oldEnoughAge) { - auto fake_track = RobotTrack(fake_vision_data, camera, team); - bot->update(fake_track); + auto fake_measurement = RobotMeasurement(fake_vision_data, camera, team); + bot->update(fake_measurement); auto msg = bot->toMsg(); // We shouldn't update if our filter is too new EXPECT_EQ(default_msg, msg); } else { // Sorry, adding a short sleep was easier than mocking the timestamp... std::this_thread::sleep_for(std::chrono::milliseconds(60)); - auto fake_track = RobotTrack(fake_vision_data, camera, team); - bot->update(fake_track); + auto fake_measurement = RobotMeasurement(fake_vision_data, camera, team); + bot->update(fake_measurement); auto msg = bot->toMsg(); // We should update if our filter is old enough EXPECT_NE(default_msg, msg); From 4eecb129dc7344d7afc9a8d538e78c578c79ecbd Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 15 Mar 2026 22:07:48 -0400 Subject: [PATCH 35/35] Ensure package name is fixed in bringup, fix iterator bug --- new_super_vision_filter/CMakeLists.txt | 2 +- new_super_vision_filter/src/vision_filter_node.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/new_super_vision_filter/CMakeLists.txt b/new_super_vision_filter/CMakeLists.txt index 5a92fa87f..661accf51 100644 --- a/new_super_vision_filter/CMakeLists.txt +++ b/new_super_vision_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(new_vision_filter) +project(new_super_vision_filter) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/new_super_vision_filter/src/vision_filter_node.cpp b/new_super_vision_filter/src/vision_filter_node.cpp index 3510df95f..47ecde238 100644 --- a/new_super_vision_filter/src/vision_filter_node.cpp +++ b/new_super_vision_filter/src/vision_filter_node.cpp @@ -164,7 +164,7 @@ class VisionFilterNode : public rclcpp::Node blue_robots.end(), [bot_measurement](const FilteredRobot & bot){return bot_measurement.getId() == bot.getId();} ); - if (it != blue_robots.end()) { + if (it == blue_robots.end()) { blue_robots.push_back( FilteredRobot(bot_measurement, team_color) ); @@ -179,7 +179,7 @@ class VisionFilterNode : public rclcpp::Node yellow_robots.end(), [bot_measurement](const FilteredRobot & bot){return bot_measurement.getId() == bot.getId();} ); - if (it != yellow_robots.end()) { + if (it == yellow_robots.end()) { yellow_robots.push_back( FilteredRobot(bot_measurement, team_color) );