diff --git a/cpp/Examples/sr_ukf_linear_function/main.cpp b/cpp/Examples/sr_ukf_linear_function/main.cpp index 2bcdeb6..df19ca1 100644 --- a/cpp/Examples/sr_ukf_linear_function/main.cpp +++ b/cpp/Examples/sr_ukf_linear_function/main.cpp @@ -16,9 +16,7 @@ #include "kalman_filter/square_root_ukf.h" static constexpr size_t DIM_X{ 2 }; -static constexpr size_t DIM_V{ 2 }; static constexpr size_t DIM_Z{ 2 }; -static constexpr size_t DIM_N{ 2 }; void runExample1(); @@ -54,18 +52,18 @@ void runExample1() P << 1.0F, 0.5F, 0.5F, 1.0F; - Matrix Q; + Matrix Q; Q << 0.5F, 0.0F, 0.0F, 0.5F; Vector z; z << 1.2F, 1.8F; - Matrix R; + Matrix R; R << 0.3F, 0.0F, 0.0F, 0.3F; - kf::SquareRootUKF srUkf; + kf::SquareRootUKF srUkf; srUkf.initialize(x, P, Q, R); srUkf.predict(funcF); diff --git a/cpp/kalman_filter/square_root_ukf.h b/cpp/kalman_filter/square_root_ukf.h index be360ff..7c09628 100644 --- a/cpp/kalman_filter/square_root_ukf.h +++ b/cpp/kalman_filter/square_root_ukf.h @@ -17,7 +17,7 @@ namespace kf { - template + template class SquareRootUKF : public KalmanFilter { public: @@ -35,7 +35,7 @@ namespace kf Matrix & matP() override { return (m_matP = m_matSk * m_matSk.transpose()); } //const Matrix & matP() const override { return (m_matP = m_matSk * m_matSk.transpose()); } - void initialize(const Vector & vecX, const Matrix & matP, const Matrix & matQ, const Matrix & matR) + void initialize(const Vector & vecX, const Matrix & matP, const Matrix & matQ, const Matrix & matR) { m_vecX = vecX; @@ -47,13 +47,13 @@ namespace kf { // cholesky factorization to get matrix Q square-root - Eigen::LLT> lltOfRv(matQ); + Eigen::LLT> lltOfRv(matQ); m_matRv = lltOfRv.matrixL(); // sqrt(Q) } { // cholesky factorization to get matrix R square-root - Eigen::LLT> lltOfRn(matR); + Eigen::LLT> lltOfRn(matR); m_matRn = lltOfRn.matrixL(); // sqrt(R) } } @@ -73,10 +73,10 @@ namespace kf /// @brief setting the cholesky factorization of process noise covariance Q. /// @param matQ process noise covariance matrix /// - void setCovarianceQ(const Matrix & matQ) + void setCovarianceQ(const Matrix & matQ) { // cholesky factorization to get matrix Q square-root - Eigen::LLT> lltOfRv(matQ); + Eigen::LLT> lltOfRv(matQ); m_matRv = lltOfRv.matrixL(); } @@ -84,10 +84,10 @@ namespace kf /// @brief setting the cholesky factorization of measurement noise covariance R. /// @param matR process noise covariance matrix /// - void setCovarianceR(const Matrix & matR) + void setCovarianceR(const Matrix & matR) { // cholesky factorization to get matrix R square-root - Eigen::LLT> lltOfRn(matQ); + Eigen::LLT> lltOfRn(matQ); m_matRn = lltOfRn.matrixL(); // sqrt(R) } @@ -182,8 +182,8 @@ namespace kf float32_t m_weighti; /// @brief unscented transform weight i for none mean samples Matrix m_matSk{ Matrix::Zero() }; /// @brief augmented state covariance (incl. process and measurement noise covariances) - Matrix m_matRv{ Matrix::Zero() }; /// @brief augmented state covariance (incl. process and measurement noise covariances) - Matrix m_matRn{ Matrix::Zero() }; /// @brief augmented state covariance (incl. process and measurement noise covariances) + Matrix m_matRv{ Matrix::Zero() }; /// @brief augmented state covariance (incl. process and measurement noise covariances) + Matrix m_matRn{ Matrix::Zero() }; /// @brief augmented state covariance (incl. process and measurement noise covariances) /// /// @brief algorithm to calculate the weights used to draw the sigma points @@ -315,13 +315,13 @@ namespace kf return matR; } - Matrix + Matrix buildCompoundMatrix( const Matrix & matSigmaX, const Vector & meanX, const Matrix & matU) { // build compoint/joint matrix for square-root covariance update - constexpr int32_t DIM_ROW{ SIGMA_DIM + DIM_V - 1 }; + constexpr int32_t DIM_ROW{ SIGMA_DIM + DIM_X - 1 }; constexpr int32_t DIM_COL{ DIM_X }; Matrix matC;