diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..3c62bf6 --- /dev/null +++ b/.clang-format @@ -0,0 +1,105 @@ +--- +Language: Cpp +# BasedOnStyle: WebKit +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: false +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterObjCDeclaration: false + AfterStruct: true + AfterUnion: true + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: All +BreakBeforeBraces: Custom +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: true +BreakConstructorInitializers: BeforeComma +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 100 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + - Regex: '.*' + Priority: 1 +IncludeIsMainRegex: '(Test)?$' +IndentCaseLabels: false +IndentWidth: 4 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 4 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 10 +PenaltyBreakBeforeFirstCallParameter: 1000 +PenaltyBreakComment: 10 +PenaltyBreakString: 10 +PenaltyExcessCharacter: 100 +PenaltyReturnTypeOnItsOwnLine: 5 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 4 +UseTab: Never diff --git a/README.md b/README.md index 05498cf..79382b3 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ [![CI_UKF](https://github.com/gabrielenava/Eigen_UKF/actions/workflows/ci_ukf.yml/badge.svg)](https://github.com/gabrielenava/Eigen_UKF/actions/workflows/ci_ukf.yml) -A C++ implementation of the Unscented Kalman Filter (UKF) using Eigen. The UKF uses a deterministic sampling technique known as the unscented transformation to calculate statistics around the mean. This technique does not require differentiability of the models. +A C++ implementation of the Unscented Kalman Filter (UKF) using Eigen. The UKF uses a deterministic sampling technique known as the Unscented Transformation to calculate statistics around the mean. This technique does not require differentiability of the models. See also https://groups.seas.harvard.edu/courses/cs281/papers/unscented.pdf for more details. @@ -28,8 +28,9 @@ make install The UKF library will be available in the `install/lib` folder. The library is composed of two classes: -- [SystemModel](lib/src/SystemModel.cpp): template class used to construct the state and observation models. Create a child class from this parent class, then edit it to add your models. -- [UnscentedKF](lib/src/UnscentedKF.cpp): the class implementing the UKF algorithm. +- [UnscentedKF](lib/src/UnscentedKF.cpp): implementation of the Unscented Kalman Filter in c++. Provides methods to set up a routine for estimation/filtering of user-defined quantities via UKF. Works alongside with the UKFModel class which provides the prediction and observation models. + +- [UKFModel](lib/src/UKFModel.cpp): defines the prediction and observation models for the UnscentedKF class. This is an abstract class: the user must create his own child class derived from UKFModel, and implement with it the prediction and observation models for his specific problem. ### Example diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 34778f0..e35821c 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -2,7 +2,7 @@ # Date: Ott. 2023 # Configure the executable -set(SRCS src/ThrustEstimationUKF.cpp src/JetSystemModel.cpp) +set(SRCS src/ThrustEstimationUKF.cpp src/JetUKFModel.cpp) add_executable(${PROJECT_NAME}_example ${SRCS}) target_include_directories(${PROJECT_NAME}_example PUBLIC include) diff --git a/example/include/JetSystemModel.h b/example/include/JetSystemModel.h deleted file mode 100644 index b40ce3f..0000000 --- a/example/include/JetSystemModel.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef JET_SYSTEM_MODEL_H -#define JET_SYSTEM_MODEL_H - -#include "SystemModel.h" - -class JetSystemModel : public SystemModel -{ -public: - // overwrite constructor for the child class - JetSystemModel(int _nMeasures, double _dt); - - // initialize variables of the child class - double dt = 0.01; - int nMeasures = 1; - - // state model parameters - static Eigen::Matrix2d A; - static Eigen::Vector2d B; - - // measurement model parameters - static Eigen::Vector2d h; - - // add additional methods and override existing methods - void configureModelParameters(); - Eigen::VectorXd dynamicsModel(const Eigen::VectorXd &x, const Eigen::VectorXd &u) override; - Eigen::VectorXd observationModel(const Eigen::VectorXd &x) override; -}; - -#endif /* end of include guard JET_SYSTEM_MODEL_H */ diff --git a/example/include/JetUKFModel.h b/example/include/JetUKFModel.h new file mode 100644 index 0000000..82c88de --- /dev/null +++ b/example/include/JetUKFModel.h @@ -0,0 +1,57 @@ +/** + * @file JetUKFModel.h + * @author Gabriele Nava + * @date 2024 + */ +#ifndef JET_UKF_MODEL_H +#define JET_UKF_MODEL_H + +#include "UKFModel.h" + +/** + * @class JetUKFModel + * @brief Inherit and overwrite methods of the UKFModel class to implement + * the prediction and observation models for jet thrust estimation. + */ +class JetUKFModel : public UKFModel +{ +public: + /** + * @brief Construct a JetUKFModel object with inputs. + * @param dt discretization time step. + */ + JetUKFModel(const double& dt); + + /** + * @brief Update the process model. + * @return None. + */ + void updateProcessModel(const Eigen::Ref u); + + /** + * @brief Get the system state at next time instant using the prediction model. + * @param x State at the current time instant; + * @return State at the next time instant. + */ + Eigen::VectorXd computePredictionFromModel(const Eigen::Ref x) override; + + /** + * @brief Get the observed variables from the current state. The observed variables are + * the ones to be compared with the available measurements. + * @param x State at current time instant; + * @return Observed variables. + */ + Eigen::VectorXd computeObservationFromModel(const Eigen::Ref x) override; + + double m_dt; + + // state model parameters + Eigen::Matrix2d m_A; + Eigen::Vector2d m_B; + Eigen::VectorXd m_u; + + // measurement model parameters + Eigen::Vector2d m_h; +}; + +#endif /* end of include guard JET_UKF_MODEL_H */ diff --git a/example/src/JetSystemModel.cpp b/example/src/JetSystemModel.cpp deleted file mode 100644 index 25508e2..0000000 --- a/example/src/JetSystemModel.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/** - * @file JetSystemModel.cpp - * @author Gabriele Nava - * @date 2023 - */ -#include - -// definition of the new constructor; initialize model parameters -JetSystemModel::JetSystemModel(int _nMeasures, double _dt) : nMeasures(_nMeasures), dt(_dt) {} -Eigen::Matrix2d JetSystemModel::A = Eigen::Matrix2d::Zero(); -Eigen::Vector2d JetSystemModel::B = Eigen::Vector2d::Zero(); -Eigen::Vector2d JetSystemModel::h = Eigen::Vector2d::Zero(); - -// definition of the child class methods -void JetSystemModel::configureModelParameters() -{ - // for jet engines, implemented a simple kinematic model - this->A << 1.0, dt, 0.0, 1.0; - this->B << std::pow(dt, 2) / 2, dt; - - // measurements are the observed thrusts - this->h << 1.0, 0.0; -} - -// overwrite the dynamicsModel class -Eigen::VectorXd JetSystemModel::dynamicsModel(const Eigen::VectorXd &x, const Eigen::VectorXd &u) -{ - return (this->A * x + this->B * u); -} - -// overwrite the observationModel class -Eigen::VectorXd JetSystemModel::observationModel(const Eigen::VectorXd &x) -{ - return this->h.transpose() * x; -} diff --git a/example/src/JetUKFModel.cpp b/example/src/JetUKFModel.cpp new file mode 100644 index 0000000..a75cc69 --- /dev/null +++ b/example/src/JetUKFModel.cpp @@ -0,0 +1,32 @@ +/** + * @file JetUKFModel.cpp + * @author Gabriele Nava + * @date 2024 + */ +#include + +JetUKFModel::JetUKFModel(const double& dt) + : m_dt(dt) +{ + // for jet engines, implemented a simple kinematic model + m_A << 1.0, m_dt, 0.0, 1.0; + m_B << std::pow(m_dt, 2) / 2, m_dt; + + // measurements are the observed thrusts + m_h << 1.0, 0.0; +} + +void JetUKFModel::updateProcessModel(const Eigen::Ref u) +{ + m_u = u; +} + +Eigen::VectorXd JetUKFModel::computePredictionFromModel(const Eigen::Ref x) +{ + return (m_A * x + m_B * m_u); +} + +Eigen::VectorXd JetUKFModel::computeObservationFromModel(const Eigen::Ref x) +{ + return m_h.transpose() * x; +} diff --git a/example/src/ThrustEstimationUKF.cpp b/example/src/ThrustEstimationUKF.cpp index 95d0a1c..42475c4 100644 --- a/example/src/ThrustEstimationUKF.cpp +++ b/example/src/ThrustEstimationUKF.cpp @@ -3,137 +3,137 @@ * @author Gabriele Nava * @date 2023 */ -#include -#include -#include +#include #include -#include +#include +#include +#include /* - This example illustrates how to use the SystemModel and UnscentedKF, the two classes composing Eigen_UKF code. - In this case, the UKF is used to estimate the thrust provided by a jet engine and its rate of change. + This example illustrates how to use UKFModel and UnscentedKF, the two classes composing Eigen_UKF + library. In this example, the UKF is used to estimate the thrust provided by a jet engine and its + rate of change. */ - -int main(int argc, char const *argv[]) +int main(int argc, char const* argv[]) { - std::cout << "Loading data..." << std::endl; - - // open the file containing the example data - std::ifstream inputFile("jetData.txt"); - - if (!inputFile.is_open()) - { - std::cerr << "Failed to open the jet data file." << std::endl; - return 1; - } - - // create variables to store the thrusts and time data - std::vector thrustData; - std::vector timeData; - std::string line; - - // read and ignore the header line - std::getline(inputFile, line); - - while (std::getline(inputFile, line)) - { - // treat line as an input string - std::istringstream iss(line); - std::string thrustStr, timeStr; - - if (std::getline(iss, thrustStr, ',') && std::getline(iss, timeStr, ',')) - { - double thrustValue = std::stod(thrustStr); - double timeValue = std::stod(timeStr); - - thrustData.push_back(thrustValue); - timeData.push_back(timeValue); - } - } - - // close the file - inputFile.close(); - - std::cout << "Data loaded!" << std::endl; - std::cout << "Running Estimation..." << std::endl; - - // step time - double dt = 0.01; - - // system initialization - int nState = 2; - int nInput = 1; - int nMeasures = 1; - - Eigen::VectorXd u(nInput); - Eigen::VectorXd x0(nState); - Eigen::MatrixXd Q(nState, nState); - Eigen::MatrixXd R(nMeasures, nMeasures); - Eigen::MatrixXd P(nState, nState); - - // set initial conditions - x0.setZero(); // initial state - Q << 0.01, 0.0, 0.0, 0.01; // process noise - R << 100.0; // measurements noise - P << 1.0, 0.0, 0.0, 1.0; // initial covariance matrix - u << 0.0; // exogenous input - - // initialize a shared pointer to an instance of the JetSystemModel class - std::shared_ptr model = std::make_shared(nMeasures, dt); - model->configureModelParameters(); - - // create an instance of the UKF class - UnscentedKF ukf(model, P, Q, R); - ukf.init(x0); - - // iterate on the data and get the estimated state - Eigen::VectorXd y(1); - std::vector x_est; - - for (int k = 0; k < timeData.size(); k++) - { - // update the measurement - y[0] = thrustData[k]; - - // call to the UKF - ukf.predict(u, dt); - ukf.update(y); - x_est.push_back(ukf.get_state()); - } - - std::cout << "Estimation done!" << std::endl; - - // saving back the estimated state to a .txt file - std::ofstream outputFile("outputUKF.txt"); - - if (!outputFile.is_open()) - { - std::cerr << "Failed to open the output file." << std::endl; - return 1; - } - - // iterate through the vector and write each Eigen vector as a row - for (const Eigen::VectorXd &row : x_est) - { - for (int i = 0; i < row.size(); ++i) - { - // write the i-th element - outputFile << row(i); - - if (i < row.size() - 1) - { - // add a delimiter between elements - outputFile << ","; - } - } - // start a new line for the next row - outputFile << "\n"; - } - - // close the output file - outputFile.close(); - - std::cout << "Results saved in outputUKF.txt" << std::endl; - - return 0; + std::cout << "Loading data..." << std::endl; + + // open the file containing the example data + std::ifstream inputFile("jetData.txt"); + + if (!inputFile.is_open()) + { + std::cerr << "Failed to open the jet data file." << std::endl; + return 1; + } + + // create variables to store the thrusts and time data + std::vector thrustData; + std::vector timeData; + std::string line; + + // read and ignore the header line + std::getline(inputFile, line); + + while (std::getline(inputFile, line)) + { + // treat line as an input string + std::istringstream iss(line); + std::string thrustStr, timeStr; + + if (std::getline(iss, thrustStr, ',') && std::getline(iss, timeStr, ',')) + { + double thrustValue = std::stod(thrustStr); + double timeValue = std::stod(timeStr); + + thrustData.push_back(thrustValue); + timeData.push_back(timeValue); + } + } + + // close the file + inputFile.close(); + + std::cout << "Data loaded!" << std::endl; + std::cout << "Running Estimation..." << std::endl; + + // step time + double dt = 0.01; + + // system initialization + int nState = 2; + int nInput = 1; + int nMeasures = 1; + + Eigen::VectorXd u(nInput); + Eigen::VectorXd x0(nState); + Eigen::MatrixXd Q(nState, nState); + Eigen::MatrixXd R(nMeasures, nMeasures); + Eigen::MatrixXd P(nState, nState); + + // set initial conditions + x0.setZero(); // initial state + Q << 0.01, 0.0, 0.0, 0.01; // process noise + R << 100.0; // measurements noise + P << 1.0, 0.0, 0.0, 1.0; // initial covariance matrix + u << 0.0; // exogenous input + + // initialize a shared pointer to an instance of the JetUKFModel class + std::shared_ptr model = std::make_shared(dt); + model->updateProcessModel(u); + + // create an instance of the UKF class + UnscentedKF ukf(model, P, Q, R); + ukf.init(x0); + + // iterate on the data and get the estimated state + Eigen::VectorXd y(1); + std::vector x_est; + + for (int k = 0; k < timeData.size(); k++) + { + // update the measurement + y[0] = thrustData[k]; + + // call to the UKF + ukf.predict(); + ukf.update(y); + x_est.push_back(ukf.getState()); + } + + std::cout << "Estimation done!" << std::endl; + + // saving back the estimated state to a .txt file + std::ofstream outputFile("outputUKF.txt"); + + if (!outputFile.is_open()) + { + std::cerr << "Failed to open the output file." << std::endl; + return 1; + } + + // iterate through the vector and write each Eigen vector as a row + for (const Eigen::VectorXd& row : x_est) + { + for (int i = 0; i < row.size(); ++i) + { + // write the i-th element + outputFile << row(i); + + if (i < row.size() - 1) + { + // add a delimiter between elements + outputFile << ","; + } + } + // start a new line for the next row + outputFile << "\n"; + } + + // close the output file + outputFile.close(); + + std::cout << "Results saved in outputUKF.txt" << std::endl; + + return 0; } diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index f1661b4..00755b2 100644 --- a/lib/CMakeLists.txt +++ b/lib/CMakeLists.txt @@ -2,6 +2,6 @@ # Date: Ott. 2023 # Configure and install the library -add_library(${PROJECT_NAME} src/UnscentedKF.cpp src/SystemModel.cpp) +add_library(${PROJECT_NAME} src/UnscentedKF.cpp src/UKFModel.cpp) target_include_directories(${PROJECT_NAME} PUBLIC include) install(TARGETS ${PROJECT_NAME} DESTINATION lib) diff --git a/lib/include/SystemModel.h b/lib/include/SystemModel.h deleted file mode 100644 index 73881f3..0000000 --- a/lib/include/SystemModel.h +++ /dev/null @@ -1,39 +0,0 @@ -/** - * @file SystemModel.h - * @author Gabriele Nava - * @date 2023 - */ -#ifndef SYSTEM_MODEL_H -#define SYSTEM_MODEL_H - -#include - -class SystemModel -{ - /* - This class is a parent class for the UKF. - The class contains two default methods: dynamicsModel and observationModel. The user can create a child class - and modify the content of dynamicsModel and observationModel methods, as well as add new methods and variables - as needed. He cannot modify the interfaces of dynamicsModel and observationModel methods, because they are needed - for the UnscentedKF class to work properly. - */ -public: - // default constructors - SystemModel(); - SystemModel(int _nMeasures); - - // default destructor - ~SystemModel(); - -protected: - // initialize class variables - int nMeasures = 1; - - // class default methods (inputs and outputs are protected to ensure compatibility with UnscentedKF class) - virtual Eigen::VectorXd dynamicsModel(const Eigen::VectorXd &x, const Eigen::VectorXd &u); - virtual Eigen::VectorXd observationModel(const Eigen::VectorXd &x); - - friend class UnscentedKF; -}; - -#endif /* end of include guard SYSTEM_MODEL_H */ diff --git a/lib/include/UKFModel.h b/lib/include/UKFModel.h new file mode 100644 index 0000000..957e0e4 --- /dev/null +++ b/lib/include/UKFModel.h @@ -0,0 +1,67 @@ +/** + * @file UKFModel.h + * @author Gabriele Nava + * @date 2024 + */ +#ifndef UKF_MODEL_H +#define UKF_MODEL_H + +#include + +/** + * @class UKFModel + * @brief Define the prediction and observation models required by the UnscentedKF class. + * This is an abstract class: the user must create his own child class derived from UKFModel, + * and implement with it the prediction and observation models for his specific problem. + */ +class UKFModel +{ +public: + /** + * @brief Construct an UKFModel object with no inputs. + */ + UKFModel(); + + /** + * @brief Construct an UKFModel object with inputs. + * @param nState size of state vector; + * @param nMeas size of measurement vector. + */ + UKFModel(const int nState, const int nMeas); + + /** + * @brief Destruct the UKFModel object. + */ + ~UKFModel(); + +protected: + /** + * @brief Update the process model. Virtual function to be overloaded by the user's child class. + * @return None. + */ + virtual void updateProcessModel(); + + /** + * @brief Get the system state at next time instant using the prediction model. + * Virtual function to be overloaded by the user's child class. + * @param x State at the current time instant; + * @return State at the next time instant. + */ + virtual Eigen::VectorXd computePredictionFromModel(const Eigen::Ref x); + + /** + * @brief Get the observed variables from the current state. The observed variables are + * the ones to be compared with the available measurements. + * Virtual function to be overloaded by the user's child class. + * @param x State at current time instant; + * @return Observed variables. + */ + virtual Eigen::VectorXd computeObservationFromModel(const Eigen::Ref x); + + int m_nState = 1; + int m_nMeas = 1; + + friend class UnscentedKF; +}; + +#endif /* end of include guard UKF_MODEL_H */ diff --git a/lib/include/UnscentedKF.h b/lib/include/UnscentedKF.h index ab435cd..ec774da 100644 --- a/lib/include/UnscentedKF.h +++ b/lib/include/UnscentedKF.h @@ -1,57 +1,108 @@ /** * @file UnscentedKF.h * @author Gabriele Nava - * @date 2023 + * @date 2024 */ #ifndef UNSCENTED_KF_H #define UNSCENTED_KF_H -#include -#include #include -#include +#include +#include +#include +/** + * @class UnscentedKF + * @brief Implementation of the Unscented Kalman Filter in c++. Provides methods to set up a routine + * for estimation/filtering of user-defined quantities via UKF. Works alongside with the UKFModel + * class which provides the prediction and observation models. + */ class UnscentedKF { - public: - // default constructor - UnscentedKF(std::shared_ptr _model, const Eigen::MatrixXd &_P, - const Eigen::MatrixXd &_Q, const Eigen::MatrixXd &_R); + /** + * @brief Construct an UnscentedKF object with no input variables. + */ + UnscentedKF(){}; + + /** + * @brief Construct an UnscentedKF object with the specified variables. + * @param model object of the SystemModel class; + * @param P initial covariance matrix; + * @param Q process noise covariance matrix; + * @param R measurements noise covariance matrix. + */ + UnscentedKF(std::shared_ptr model, + const Eigen::Ref P, + const Eigen::Ref Q, + const Eigen::Ref R); + + /** + * @brief Destruct the UnscentedKF object. + */ + ~UnscentedKF(); - // default destructor - ~UnscentedKF(); + /** + * @brief Initialize the UnscentedKF object. Initial state is set to zero. + * @return None. + */ + void init(); - void init(); - void init(const Eigen::VectorXd &x); - void predict(const Eigen::VectorXd &u, const double &dt); - void update(const Eigen::VectorXd &y); + /** + * @brief Initialize the UnscentedKF object. + * @param x initial state. + * @return None. + */ + void init(const Eigen::VectorXd& x); - Eigen::VectorXd get_state(); - Eigen::MatrixXd get_cov(); + /** + * @brief Predict the system state using the prediction model provided by UKFModel class. + * @return None. + */ + void predict(); + + /** + * @brief Update the state with the observation model provided by the UKMModel class + * and corrections provided by real measurements. + * @param y measurements vector + * @return None. + */ + void update(const Eigen::VectorXd& y); + + /** + * @brief Get the current state estimated by the UKF. + * @return the state estimated by the UKF. + */ + Eigen::VectorXd getState(); + + /** + * @brief Get the current state covariance estimated by the UKF. + * @return the covariance matrix estimated by the UKF. + */ + Eigen::MatrixXd getCov(); private: - // internal variable and methods - void ut(const Eigen::VectorXd &x, const Eigen::MatrixXd &P, std::vector &sigmaPt); - - std::vector wc, ws; // weights. - std::shared_ptr model; - - Eigen::MatrixXd P; - Eigen::MatrixXd Q; - Eigen::MatrixXd R; - Eigen::MatrixXd P0; // initial P. - Eigen::MatrixXd K; // kalman gain. - Eigen::MatrixXd I; // unit matrix. - Eigen::VectorXd x_hat; // estimated state. - - // internal parameters (not supposed to be edited) - double alpha = 0.001; - double beta = 2.0; - double k = 0.0; - - // lambda parameter (to be updated by the constructor) - double lambda = 0.0; + // implementation of the Unscented Transform + void + ut(const Eigen::VectorXd& x, const Eigen::MatrixXd& P, std::vector& sigmaPt); + + std::shared_ptr m_model; // pointer to the prediction and measurements models + + std::vector m_wc, m_ws; // weights + + Eigen::MatrixXd m_P; // state covariance + Eigen::MatrixXd m_Q; // process noise covariance + Eigen::MatrixXd m_R; // measurements noise covariance + Eigen::MatrixXd m_P0; // initial P + Eigen::MatrixXd m_K; // kalman gain + Eigen::MatrixXd m_I; // unit matrix + Eigen::VectorXd m_x_hat; // estimated state + + // internal parameters (not supposed to be edited) + double m_alpha = 0.001; + double m_beta = 2.0; + double m_k = 0.0; + double m_lambda = 0.0; }; #endif /* end of include guard UNSCENTED_KF_H */ diff --git a/lib/src/SystemModel.cpp b/lib/src/SystemModel.cpp deleted file mode 100644 index 1b88488..0000000 --- a/lib/src/SystemModel.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/** - * @file SystemModel.cpp - * @author Gabriele Nava - * @date 2023 - */ -#include - -// define the constructors and destructor -SystemModel::SystemModel() {} -SystemModel::SystemModel(int _nMeasures) : nMeasures(_nMeasures) {} -SystemModel::~SystemModel() {} - -Eigen::VectorXd SystemModel::dynamicsModel(const Eigen::VectorXd &x, const Eigen::VectorXd &u) -{ - // default behaviour: return a vector of zeros, of the same length of x - Eigen::VectorXd x_upd(x.size()); - x_upd.setZero(); - return x_upd; -} - -Eigen::VectorXd SystemModel::observationModel(const Eigen::VectorXd &x) -{ - // default behaviour: return a vector of zeros, of the same length of x - Eigen::VectorXd y(this->nMeasures); - y.setZero(); - return y; -} diff --git a/lib/src/UKFModel.cpp b/lib/src/UKFModel.cpp new file mode 100644 index 0000000..c0b778b --- /dev/null +++ b/lib/src/UKFModel.cpp @@ -0,0 +1,36 @@ +/** + * @file UKFModel.cpp + * @author Gabriele Nava + * @date 2024 + */ +#include + +UKFModel::UKFModel() +{ +} + +UKFModel::UKFModel(const int nState, const int nMeas) + : m_nState(nState) + , m_nMeas(nMeas) +{ +} + +UKFModel::~UKFModel() +{ +} + +void UKFModel::updateProcessModel() +{ +} + +Eigen::VectorXd UKFModel::computePredictionFromModel(const Eigen::Ref x) +{ + return x; +} + +Eigen::VectorXd UKFModel::computeObservationFromModel(const Eigen::Ref x) +{ + static Eigen::MatrixXd h(m_nMeas, m_nState); + h.setZero(); + return h * x; +} diff --git a/lib/src/UnscentedKF.cpp b/lib/src/UnscentedKF.cpp index 2487545..d584338 100644 --- a/lib/src/UnscentedKF.cpp +++ b/lib/src/UnscentedKF.cpp @@ -1,147 +1,159 @@ /** * @file UnscentedKF.cpp * @author Gabriele Nava - * @date 2023 + * @date 2024 */ #include -UnscentedKF::UnscentedKF(std::shared_ptr _model, const Eigen::MatrixXd &_P, const Eigen::MatrixXd &_Q, - const Eigen::MatrixXd &_R) : model(_model), P(_P), Q(_Q), R(_R), P0(_P), x_hat(_P.rows()) +UnscentedKF::UnscentedKF(std::shared_ptr model, + const Eigen::Ref P, + const Eigen::Ref Q, + const Eigen::Ref R) + : m_model(model) + , m_P(P) + , m_Q(Q) + , m_R(R) + , m_P0(P) + , m_x_hat(P.rows()) { - // calculate lambda parameter - lambda = std::pow(alpha, 2) * (P.rows() + k) - P.rows(); - - // reserve memory for weights vectors - wc.reserve(2 * P.rows() + 1); - ws.reserve(2 * P.rows() + 1); - - // populate weights vectors - ws.push_back(lambda / (P.rows() + lambda)); - wc.push_back(ws[0] + (1.0 - std::pow(alpha, 2) + beta)); - - for (size_t i = 1; i < 2 * P.rows() + 1; i++) - { - double val = 1.0 / (2.0 * (lambda + P.rows())); - ws.push_back(val); - wc.push_back(val); - } + // calculate lambda parameter + m_lambda = std::pow(m_alpha, 2) * (m_P.rows() + m_k) - m_P.rows(); + + // reserve memory for weights vectors + m_wc.reserve(2 * m_P.rows() + 1); + m_ws.reserve(2 * m_P.rows() + 1); + + // populate weights vectors + m_ws.emplace_back(m_lambda / (m_P.rows() + m_lambda)); + m_wc.emplace_back(m_ws[0] + (1.0 - std::pow(m_alpha, 2) + m_beta)); + + for (size_t i = 1; i < 2 * m_P.rows() + 1; i++) + { + double val = 1.0 / (2.0 * (m_lambda + m_P.rows())); + m_ws.emplace_back(val); + m_wc.emplace_back(val); + } } -UnscentedKF::~UnscentedKF() {} +UnscentedKF::~UnscentedKF() +{ +} void UnscentedKF::init() { - // set initial values. Initial state set to zero - P = P0; - x_hat.setZero(); - I = Eigen::MatrixXd::Identity(P.rows(), P.rows()); + // set initial values. Initial state set to zero + m_P = m_P0; + m_x_hat.setZero(); + m_I = Eigen::MatrixXd::Identity(m_P.rows(), m_P.rows()); } -void UnscentedKF::init(const Eigen::VectorXd &x) +void UnscentedKF::init(const Eigen::VectorXd& x) { - // set initial values. Initial state passed by the user - init(); - x_hat = x; + // set initial values. Initial state passed by the user + init(); + m_x_hat = x; } -void UnscentedKF::ut(const Eigen::VectorXd &x, const Eigen::MatrixXd &P, std::vector &sigmaPt) +void UnscentedKF::ut(const Eigen::VectorXd& x, + const Eigen::MatrixXd& P, + std::vector& sigmaPt) { - // Calculate square root of P, i.e, P = L*L^t - Eigen::LLT chol(P); - Eigen::MatrixXd L = chol.matrixL(); - - sigmaPt.clear(); - sigmaPt.reserve(2 * P.rows() + 1); - - // initialize sigma points with current estimate - sigmaPt.push_back(x_hat); - - for (size_t i = 0; i < P.rows(); i++) - { - // generate sigma points - Eigen::VectorXd preSigmaPt1 = x + (sqrt(P.rows() + lambda) * L).col(i); - Eigen::VectorXd preSigmaPt2 = x - (sqrt(P.rows() + lambda) * L).col(i); - sigmaPt.push_back(preSigmaPt1); - sigmaPt.push_back(preSigmaPt2); - } + // Calculate square root of P, i.e, P = L*L^t + Eigen::LLT chol(P); + Eigen::MatrixXd L = chol.matrixL(); + + sigmaPt.clear(); + sigmaPt.reserve(2 * P.rows() + 1); + + // initialize sigma points with current estimate + sigmaPt.emplace_back(m_x_hat); + + for (size_t i = 0; i < P.rows(); i++) + { + // generate sigma points + Eigen::VectorXd preSigmaPt1 = x + (sqrt(P.rows() + m_lambda) * L).col(i); + Eigen::VectorXd preSigmaPt2 = x - (sqrt(P.rows() + m_lambda) * L).col(i); + sigmaPt.emplace_back(preSigmaPt1); + sigmaPt.emplace_back(preSigmaPt2); + } } -void UnscentedKF::predict(const Eigen::VectorXd &u, const double &dt) +void UnscentedKF::predict() { - // use the model to predict the value of the state at (k+1) - - // generate sigma points using unscented transform (ut) - std::vector sigmaPt; - ut(x_hat, P, sigmaPt); - - // predict the new state using the dynamic model - Eigen::VectorXd x_new(x_hat.rows()); - x_new.setZero(); - - for (size_t i = 0; i < 2 * P.rows() + 1; i++) - { - // propagate the sigma points using the dynamics - sigmaPt[i] = model->dynamicsModel(sigmaPt[i], u); - x_new += ws[i] * sigmaPt[i]; - } - - // update the covariance matrix - Eigen::MatrixXd P_new(P.rows(), P.rows()); - P_new.setZero(); - - for (size_t i = 0; i < 2 * P.rows() + 1; i++) - { - P_new += wc[i] * (sigmaPt[i] - x_new) * (sigmaPt[i] - x_new).transpose(); - } - - x_hat = x_new; - P = P_new + Q; + // use the model to predict the value of the state at (k+1) + + // generate sigma points using unscented transform (ut) + std::vector sigmaPt; + ut(m_x_hat, m_P, sigmaPt); + + // predict the new state using the dynamic model + Eigen::VectorXd x_new(m_x_hat.rows()); + x_new.setZero(); + + for (size_t i = 0; i < 2 * m_P.rows() + 1; i++) + { + // propagate the sigma points using the dynamics + sigmaPt[i] = m_model->computePredictionFromModel(sigmaPt[i]); + x_new += m_ws[i] * sigmaPt[i]; + } + + // update the covariance matrix + Eigen::MatrixXd P_new(m_P.rows(), m_P.rows()); + P_new.setZero(); + + for (size_t i = 0; i < 2 * m_P.rows() + 1; i++) + { + P_new += m_wc[i] * (sigmaPt[i] - x_new) * (sigmaPt[i] - x_new).transpose(); + } + + m_x_hat = x_new; + m_P = P_new + m_Q; } -void UnscentedKF::update(const Eigen::VectorXd &y) +void UnscentedKF::update(const Eigen::VectorXd& y) { - // use the measurements to correct the prediction - - // generate sigma points using unscented transform (ut) - std::vector sigmaPt; - ut(x_hat, P, sigmaPt); - - std::vector gamma; - gamma.resize(sigmaPt.size()); - - // compute the observed quantities via observation model - Eigen::VectorXd z(y.rows()); - z.setZero(); - - for (size_t i = 0; i < 2 * P.rows() + 1; i++) - { - gamma[i] = model->observationModel(sigmaPt[i]); - z += ws[i] * gamma[i]; - } - - // update the covariance matrices - Eigen::MatrixXd P_obs(z.rows(), z.rows()), P_est(P.rows(), z.rows()); - P_obs.setZero(); - P_est.setZero(); - - for (size_t i = 0; i < 2 * P.rows() + 1; i++) - { - P_obs += wc[i] * (gamma[i] - z) * (gamma[i] - z).transpose(); - P_est += wc[i] * (sigmaPt[i] - x_hat) * (gamma[i] - z).transpose(); - } - - // compute the Kalman gain and estimate the state and covariance - K = P_est * (P_obs + R).inverse(); - x_hat += K * (y - z); - P = P - K * (P_obs + R) * K.transpose(); + // use the measurements and observation model to correct the prediction + + // generate sigma points using unscented transform (ut) + std::vector sigmaPt; + ut(m_x_hat, m_P, sigmaPt); + + std::vector gamma; + gamma.resize(sigmaPt.size()); + + // compute the observed quantities via observation model + Eigen::VectorXd z(y.rows()); + z.setZero(); + + for (size_t i = 0; i < 2 * m_P.rows() + 1; i++) + { + gamma[i] = m_model->computeObservationFromModel(sigmaPt[i]); + z += m_ws[i] * gamma[i]; + } + + // update the covariance matrices + Eigen::MatrixXd P_obs(z.rows(), z.rows()), P_est(m_P.rows(), z.rows()); + P_obs.setZero(); + P_est.setZero(); + + for (size_t i = 0; i < 2 * m_P.rows() + 1; i++) + { + P_obs += m_wc[i] * (gamma[i] - z) * (gamma[i] - z).transpose(); + P_est += m_wc[i] * (sigmaPt[i] - m_x_hat) * (gamma[i] - z).transpose(); + } + + // compute the Kalman gain and estimate the state and covariance + m_K = P_est * (P_obs + m_R).inverse(); + m_x_hat += m_K * (y - z); + m_P = m_P - m_K * (P_obs + m_R) * m_K.transpose(); } -Eigen::VectorXd UnscentedKF::get_state() +Eigen::VectorXd UnscentedKF::getState() { - return x_hat; + return m_x_hat; } -Eigen::MatrixXd UnscentedKF::get_cov() +Eigen::MatrixXd UnscentedKF::getCov() { - return P; + return m_P; } diff --git a/test/testUKF.cpp b/test/testUKF.cpp index 54cee1b..6b082f7 100644 --- a/test/testUKF.cpp +++ b/test/testUKF.cpp @@ -3,57 +3,54 @@ * @author Gabriele Nava * @date 2023 */ -#include +#include #include -#include +#include TEST_CASE("UKF TEST") { - // step time - double dt = 0.01; - - // system initialization - int nState = 2; - int nInput = 1; - int nMeasures = 1; - - Eigen::VectorXd u(nInput); - Eigen::VectorXd x0(nState); - Eigen::VectorXd y(nMeasures); - Eigen::MatrixXd Q(nState, nState); - Eigen::MatrixXd R(nMeasures, nMeasures); - Eigen::MatrixXd P(nState, nState); - - // set initial conditions - x0.setZero(); // initial state - Q.setIdentity(); // process noise - R.setIdentity(); // measurements noise - P.setIdentity(); // initial covariance matrix - u.setZero(); // exogenous input - y.setZero(); // measurements - - // initialize a shared pointer to an instance of the JetSystemModel class - std::shared_ptr model = std::make_shared(nMeasures); - - // create an instance of the UKF class - UnscentedKF ukf(model, P, Q, R); - ukf.init(x0); - - // get the first estimated state - Eigen::VectorXd x_est(nState); - - ukf.predict(u, dt); - ukf.update(y); - x_est = ukf.get_state(); - - // use ctest --output-on-failure to visualize this message in case of failure - INFO("UKF output is: " << x_est); - - // expected solution - Eigen::VectorXd check_solution; - check_solution.resize(nState); - check_solution.setZero(); - - // check that computed values match expected values - REQUIRE(x_est.isApprox(check_solution, 0.001)); + // system initialization + int nState = 2; + int nInput = 1; + int nMeasures = 1; + + Eigen::VectorXd u(nInput); + Eigen::VectorXd x0(nState); + Eigen::VectorXd y(nMeasures); + Eigen::MatrixXd Q(nState, nState); + Eigen::MatrixXd R(nMeasures, nMeasures); + Eigen::MatrixXd P(nState, nState); + + // set initial conditions + x0.setZero(); // initial state + Q.setIdentity(); // process noise + R.setIdentity(); // measurements noise + P.setIdentity(); // initial covariance matrix + u.setZero(); // exogenous input + y.setZero(); // measurements + + // initialize a shared pointer to an instance of the JetSystemModel class + std::shared_ptr model = std::make_shared(nState, nMeasures); + + // create an instance of the UKF class + UnscentedKF ukf(model, P, Q, R); + ukf.init(x0); + + // get the first estimated state + Eigen::VectorXd x_est(nState); + + ukf.predict(); + ukf.update(y); + x_est = ukf.getState(); + + // use ctest --output-on-failure to visualize this message in case of failure + INFO("UKF output is: " << x_est); + + // expected solution + Eigen::VectorXd check_solution; + check_solution.resize(nState); + check_solution.setZero(); + + // check that computed values match expected values + REQUIRE(x_est.isApprox(check_solution, 0.001)); }