diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp index 5fa5550bf1..cbe9be9069 100644 --- a/example/kalman/ukf-nonlinear-complex-example.cpp +++ b/example/kalman/ukf-nonlinear-complex-example.cpp @@ -540,7 +540,7 @@ int main(/*const int argc, const char *argv[]*/) const unsigned int nbCmds = cmds.size(); // Initialize the attributes of the UKF - std::shared_ptr drawer = std::make_shared(3, 0.00001, 2., 0, stateResidual, stateAdd); + std::shared_ptr drawer = std::make_shared(3, 0.1, 2., 0, stateResidual, stateAdd); vpMatrix R1landmark(2, 2, 0.); // The covariance of the noise introduced by the measurement with 1 landmark R1landmark[0][0] = sigmaRange*sigmaRange; diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h index cd0039da9d..de2e9ba1b8 100644 --- a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h @@ -135,7 +135,7 @@ class VISP_EXPORT vpUKSigmaDrawerMerwe : public vpUKSigmaDrawerAbstract protected: inline void computeLambda() { - m_lambda = m_alpha * m_alpha * (m_n + m_kappa) - m_n; + m_lambda = m_alpha * m_alpha * (static_cast(m_n) + m_kappa) - static_cast(m_n); } double m_alpha; /*!< A factor, which should be a real in the interval [0; 1]. The larger alpha is, diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index 219763ed59..d5752b2d46 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -472,7 +472,7 @@ def measureWithNoise(self, pos: ColVector) -> ColVector: robot = vpBicycleModel(wheelbase) # The object that draws the sigma points used by the UKF - drawer = UKSigmaDrawerMerwe(n=3, alpha=0.00001, beta=2, kappa=0, resFunc=residual_state_vectors, addFunc=add_state_vectors) + drawer = UKSigmaDrawerMerwe(n=3, alpha=0.1, beta=2, kappa=0, resFunc=residual_state_vectors, addFunc=add_state_vectors) # The matrices require for the construction of the Unscented filter P0 = Matrix([[0.1, 0., 0.], @@ -482,7 +482,6 @@ def measureWithNoise(self, pos: ColVector) -> ColVector: R = Matrix(2*nbLandmarks, 2 * nbLandmarks) # The measurement covariance matrix for the grid of landmarks for i in range(nbLandmarks): R.insert(R1landmark, 2*i, 2*i) - print(R) Q = Matrix() # The process covariance matrix Q.eye(3)