From e9ccece818da9ef6f5a30d3731fd77af3e18443c Mon Sep 17 00:00:00 2001 From: rlagneau Date: Wed, 22 May 2024 07:52:14 +0200 Subject: [PATCH 01/16] [EXAMPLE] [PYTHON] Updated copyright --- modules/python/examples/ukf-linear-example.py | 6 ++---- modules/python/examples/ukf-nonlinear-example.py | 4 ++-- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/modules/python/examples/ukf-linear-example.py b/modules/python/examples/ukf-linear-example.py index 1e27fde959..1121491e8f 100644 --- a/modules/python/examples/ukf-linear-example.py +++ b/modules/python/examples/ukf-linear-example.py @@ -1,7 +1,7 @@ ############################################################################# # # ViSP, open source Visual Servoing Platform software. -# Copyright (C) 2005 - 2023 by Inria. All rights reserved. +# Copyright (C) 2005 - 2024 by Inria. All rights reserved. # # This software is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by @@ -58,7 +58,7 @@ not perfect. """ -from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, GaussRand +from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe # For the Graphical User Interface try: @@ -184,8 +184,6 @@ def generate_Q_matrix(dt: float) -> Matrix: z_prec = ColVector(2, 0.) # Previous measurement vector np.random.seed(4224) - rngX = GaussRand(sigmaXmeas, 0., 4224) # Gaussian noise random generator for x-axis measurement - rngY = GaussRand(sigmaYmeas, 0., 2112) # Gaussian noise random generator for y-axis measurement for i in range(100): # Creating noisy measurements x_meas = gt_X[0] + np.random.normal(0.0, sigmaXmeas) diff --git a/modules/python/examples/ukf-nonlinear-example.py b/modules/python/examples/ukf-nonlinear-example.py index 5f10197dee..65cdfff282 100644 --- a/modules/python/examples/ukf-nonlinear-example.py +++ b/modules/python/examples/ukf-nonlinear-example.py @@ -1,7 +1,7 @@ ############################################################################# # # ViSP, open source Visual Servoing Platform software. -# Copyright (C) 2005 - 2023 by Inria. All rights reserved. +# Copyright (C) 2005 - 2024 by Inria. All rights reserved. # # This software is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by @@ -63,7 +63,7 @@ not perfect. """ -from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, GaussRand, Math +from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, Math import numpy as np from typing import List from math import cos, sin From 69ed12d1e88fd4d6e821826fb7cf1b4ddf421b3b Mon Sep 17 00:00:00 2001 From: rlagneau Date: Wed, 22 May 2024 07:53:07 +0200 Subject: [PATCH 02/16] [WIP][EXAMPLE][PYTHON] Added the UKF non-linear complex example in Python. Problem with Cholesky's inversion --- .../examples/ukf-nonlinear-complex-example.py | 537 ++++++++++++++++++ 1 file changed, 537 insertions(+) create mode 100644 modules/python/examples/ukf-nonlinear-complex-example.py diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py new file mode 100644 index 0000000000..219763ed59 --- /dev/null +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -0,0 +1,537 @@ +############################################################################# +# +# ViSP, open source Visual Servoing Platform software. +# Copyright (C) 2005 - 2024 by Inria. All rights reserved. +# +# This software is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# See the file LICENSE.txt at the root directory of this source +# distribution for additional information about the GNU GPL. +# +# For unp.sing ViSP with software that can not be combined with the GNU +# GPL, please contact Inria about acquiring a ViSP Professional +# Edition License. +# +# See https://visp.inria.fr for more information. +# +# This software was developed at: +# Inria Rennes - Bretagne Atlantique +# Campus Universitaire de Beaulieu +# 35042 Rennes Cedex +# France +# +# If you have questions regarding the use of this file, please contact +# Inria at visp@inria.fr +# +# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE +# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. +# +# Description: +# ViSP Python example to simulate an image-based visual servo on four 2D points +# +############################################################################# + +""" @example ukf-nonlinear-complex-example.py + Example of a complex non-linear use-case of the Unscented Kalman Filter (UKF). + The system we are interested in is a 4-wheel robot, moving at a low velocity. + As such, it can be modeled unp.sing a bicycle model. + + The state vector of the UKF is: + \f{eqnarray*}{ + \textbf{x}[0] &=& x \\ + \textbf{x}[1] &=& y \\ + \textbf{x}[2] &=& \theta + \f} + where \f$ \theta \f$ is the heading of the robot. + + The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the + robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark + along the x and y axis, the measurement vector can be written as: + \f{eqnarray*}{ + \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ + \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta + \f} + + Some noise is added to the measurement vector to simulate measurements which are + not perfect. + + The mean of several angles must be computed in the Unscented Transform. The definition we chose to use + is the following: + + \f$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n \np.sin{\theta_i}}{n}, \frac{\sum_{i=1}^n \np.cos{\theta_i}}{n}) \f$ + + As the Unscented Transform uses a weighted mean, the actual implementation of the weighted mean + of several angles is the following: + + \f$ mean_{weighted}(\boldsymbol{\theta}) = atan2 (\sum_{i=1}^n w_m^i \np.sin{\theta_i}, \sum_{i=1}^n w_m^i \np.cos{\theta_i}) \f$ + + where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. + + Additionnally, the addition and substraction of angles must be carefully done, as the result + must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use + the interval \f$[- \pi ; \pi ]\f$ . +""" + +from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, Math +import numpy as np +from typing import List +from math import sqrt + +# For the Graphical User Interface +try: + from visp.gui import Plot + has_gui = True +except: + has_gui = False +import numpy as np + +def normalizeAngle(angle: float) -> float: + angleIn0to2pi = Math.modulo(angle, 2. * np.pi) + angleInMinPiPi = angleIn0to2pi + if angleInMinPiPi > np.pi: + # Substract 2 PI to be in interval [-Pi; Pi] + angleInMinPiPi = angleInMinPiPi - 2. * np.pi + return angleInMinPiPi + +def measurementMean(measurements: List[ColVector], wm: List[float]) -> ColVector: + """ + @brief Compute the weighted mean of measurement vectors. + + @param[in] measurements The measurement vectors, such as v[0] = dist_0 ; v[1] = bearing_0; + v[2] = dist_1 ; v[3] = bearing_1 ... + @param[in] wm The associated weights. + + @return ColVector The weighted mean. + """ + nbPoints = len(measurements) + sizeMeasurement = measurements[0].size() + nbLandmarks = sizeMeasurement // 2 + mean = np.zeros(sizeMeasurement) + sumCos = np.zeros(nbLandmarks) + sumSin = np.zeros(nbLandmarks) + + for i in range(nbPoints): + for j in range(nbLandmarks): + mean[2*j] += wm[i] * measurements[i][2*j] + sumCos[j] += np.cos(measurements[i][(2*j)+1]) * wm[i] + sumSin[j] += np.sin(measurements[i][(2*j)+1]) * wm[i] + + for j in range(nbLandmarks): + mean[(2*j)+1] = np.arctan2(sumSin[j], sumCos[j]) + return ColVector(mean) + +def measurementResidual(meas: ColVector, toSubstract: ColVector) -> ColVector: + """ + @brief Compute the substraction between two vectors expressed in the measurement space, + such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... + + @param[in] meas Measurement to which we must substract something. + @param[in] toSubstract The something we must substract. + + @return ColVector \b meas - \b toSubstract . + """ + nbMeasures = meas.size() + nbPoints = int(nbMeasures / 2) + res = np.zeros(nbMeasures) + for i in range(nbPoints): + res[2*i] = meas[2*i] - toSubstract[2*i] + res[2*i+1] = normalizeAngle(meas[2*i + 1] - toSubstract[2*i + 1]) + return ColVector(res) + +def add_state_vectors(a: ColVector, b: ColVector) -> ColVector: + """ + @brief Compute the addition between two vectors expressed in the state space, + such as v[0] = x ; v[1] = y; v[2] = heading . + + @param a The first state vector to which another state vector must be added. + @param b The other state vector that must be added to a. + + @return ColVector The sum a + b. + """ + add = a + b + return ColVector([add[0], add[1], normalizeAngle(add[2])] ) + + +def mean_state_vectors(states: List[ColVector], wm: List[float]) -> ColVector: + """ + @brief Compute the weighted mean of state vectors. + + @param states The state vectors. + @param wm The associated weights. + @return ColVector The weighted mean. + """ + mean = np.zeros(3) + nbPoints = len(states) + sumCos = 0. + sumSin = 0. + for i in range (nbPoints): + mean[0] += wm[i] * states[i][0] + mean[1] += wm[i] * states[i][1] + sumCos += wm[i] * np.cos(states[i][2]) + sumSin += wm[i] * np.sin(states[i][2]) + + mean[2] = np.arctan2(sumSin, sumCos) + return ColVector(mean) + +def residual_state_vectors(a, b) -> ColVector: + """ + @brief Compute the substraction between two vectors expressed in the state space, + such as v[0] = x ; v[1] = y; v[2] = heading . + + @param a The first state vector to which another state vector must be substracted. + @param b The other state vector that must be substracted to a. + + @return ColVector The substraction a - b. + """ + res = a - b + return ColVector([res[0], res[1], normalizeAngle(res[2])]) + +def fx(x: ColVector, dt: float) -> ColVector: + """ + @brief As the state model {x, y, \f$ \theta \f$} does not contain any velocity + information, it does not evolve without commands. + + @param x The internal state of the UKF. + @param dt The sampling time: how far in the future are we projecting x. + + @return ColVector The updated internal state, projected in time, also known as the prior. + """ + return x + +def generateTurnCommands(v: float, angleStart: float, angleStop: float, nbSteps: int) -> List[ColVector]: + """ + @brief Compute the commands realinp.sing a turn at constant linear velocity. + + @param v Constant linear velocity. + @param angleStart Starting angle (in degrees). + @param angleStop Stop angle (in degrees). + @param nbSteps Number of steps to perform the turn. + @return List[ColVector] The corresponding list of commands. + """ + cmds = [] + dTheta = Math.rad(angleStop - angleStart) / float(nbSteps - 1); + for i in range(nbSteps): + theta = Math.rad(angleStart) + dTheta * float(i); + cmd = ColVector([v, theta]) + cmds.append(cmd) + return cmds + +def generateCommands() -> List[ColVector]: + """ + @brief Generate the list of commands for the simulation. + + @return List[ColVector] The list of commands to use in the simulation + """ + cmds = [] + # Starting by an straight line acceleration + nbSteps = 30 + dv = (1.1 - 0.001) / float(nbSteps - 1) + for i in range(nbSteps): + cmd = ColVector([0.001 + float(i) * dv, 0.]) + cmds.append(cmd) + + # Left turn + lastLinearVelocity = cmds[len(cmds) -1][0] + leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 2, 15) + cmds.extend(leftTurnCmds) + for i in range(100): + cmds.append(cmds[len(cmds) -1]) + + # Right turn + lastLinearVelocity = cmds[len(cmds) -1][0] + rightTurnCmds = generateTurnCommands(lastLinearVelocity, 2, -2, 15); + cmds.extend(rightTurnCmds) + for i in range(200): + cmds.append(cmds[len(cmds) -1]) + + # Left turn again + lastLinearVelocity = cmds[len(cmds) -1][0] + leftTurnCmds = generateTurnCommands(lastLinearVelocity, -2, 0, 15) + cmds.extend(leftTurnCmds) + for i in range(150): + cmds.append(cmds[len(cmds) -1]) + + lastLinearVelocity = cmds[len(cmds) -1][0] + leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 1, 25) + cmds.extend(leftTurnCmds) + for i in range(150): + cmds.append(cmds[len(cmds) -1]) + + return cmds + +class vpBicycleModel: + """ + @brief Class that approximates a 4-wheel robot unp.sing a bicycle model. + """ + def __init__(self, w: float): + """ + @brief Construct a new vpBicycleModel object. + + @param w The length of the wheelbase. + """ + self.m_w = w # The length of the wheelbase. + + def computeMotion(self, u: ColVector, x: ColVector, dt: float) -> ColVector: + """ + @brief Models the effect of the command on the state model. + + @param u The commands. u[0] = velocity ; u[1] = steeringAngle . + @param x The state model. x[0] = x ; x[1] = y ; x[2] = heading + @param dt The period. + @return ColVector The state model after applying the command. + """ + heading = x[2] + vel = u[0] + steeringAngle = u[1] + distance = vel * dt + + if (abs(steeringAngle) > 0.001): + # The robot is turning + beta = (distance / self.m_w) * np.tan(steeringAngle) + radius = self.m_w / np.tan(steeringAngle); + np.sinh = np.sin(heading) + np.sinhb = np.sin(heading + beta) + np.cosh = np.cos(heading) + np.coshb = np.cos(heading + beta) + motion = ColVector([ + -radius * np.sinh + radius * np.sinhb, + radius * np.cosh - radius * np.coshb, + beta + ]) + return motion + else: + # The robot is moving in straight line + motion = ColVector([ + distance * np.cos(heading), + distance * np.sin(heading), + 0. + ]) + return motion + + def move(self, u: ColVector, x: ColVector, dt: float) -> ColVector: + """ + @brief Models the effect of the command on the state model. + + @param u The commands. u[0] = velocity ; u[1] = steeringAngle . + @param x The state model. x[0] = x ; x[1] = y ; x[2] = heading + @param dt The period. + @return ColVector The state model after applying the command. + """ + motion = self.computeMotion(u, x, dt) + newX = x + motion + normalizedAngle = normalizeAngle(newX[2]) + return ColVector([newX[0], newX[1], normalizedAngle]) + +class vpLandmarkMeasurements: + """ + @brief Class that permits to convert the position + heading of the 4-wheel + robot into measurements from a landmark. + """ + def __init__(self, x: float, y: float, range_std: float, rel_angle_std: float): + """ + @brief Construct a new vpLandmarkMeasurements object. + + @param x The position along the x-axis of the landmark. + @param y The position along the y-axis of the landmark. + @param range_std The standard deviation of the range measurements. + @param rel_angle_std The standard deviation of the relative angle measurements. + """ + self.m_x = x # The position along the x-axis of the landmark + self.m_y = y # The position along the y-axis of the landmark + self.m_range_std = range_std # The standard deviation of the range measurement + self.m_rel_angle_std = rel_angle_std # The standard deviation of the relative angle measurement + np.random.seed(4224) + + def state_to_measurement(self, chi: ColVector) -> ColVector: + """ + @brief Convert the prior of the UKF into the measurement space. + + @param chi The prior. + @return ColVector The prior expressed in the measurement space. + """ + dx = self.m_x - chi[0] + dy = self.m_y - chi[1] + meas = ColVector([sqrt(dx * dx + dy * dy), normalizeAngle(np.arctan2(dy, dx) - chi[2])]) + return meas + + def measureGT(self, pos: ColVector) -> ColVector: + """ + @brief Perfect measurement of the range and relative orientation of the robot + located at pos. + + @param pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + @return ColVector [0] the range [1] the relative orientation of the robot. + """ + dx = self.m_x - pos[0] + dy = self.m_y - pos[1] + range = sqrt(dx * dx + dy * dy) + orientation = normalizeAngle(np.arctan2(dy, dx) - pos[2]) + measurements = ColVector([range, orientation]) + return measurements + + def measureWithNoise(self, pos: ColVector) -> ColVector: + """ + @brief Noisy measurement of the range and relative orientation that + correspond to pos. + + @param pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + @return ColVector [0] the range [1] the relative orientation. + """ + measurementsGT = self.measureGT(pos) + measurementsNoisy = measurementsGT + range = measurementsNoisy[0] + np.random.normal(0., self.m_range_std) + relAngle = normalizeAngle(measurementsNoisy[1] + np.random.normal(0., self.m_rel_angle_std)) + return ColVector([range, relAngle]) + + +class vpLandmarksGrid: + """ + @brief Class that represent a grid of landmarks that measure the distance and + relative orientation of the 4-wheel robot. + """ + def __init__(self, landmarks: List[vpLandmarkMeasurements]): + """ + @brief Construct a new vpLandmarksGrid object. + + @param landmarks The list of landmarks forming the grid. + """ + self.m_landmarks = landmarks # The list of landmarks forming the grid. + + def state_to_measurement(self, chi: ColVector) -> ColVector: + """ + @brief Convert the prior of the UKF into the measurement space. + + @param chi The prior. + @return ColVector The prior expressed in the measurement space. + """ + nbLandmarks = len(self.m_landmarks) + measurements = np.zeros(2*nbLandmarks) + for i in range (nbLandmarks): + landmarkMeas = self.m_landmarks[i].state_to_measurement(chi) + measurements[2*i] = landmarkMeas[0] + measurements[(2*i) + 1] = landmarkMeas[1] + return ColVector(measurements) + + def measureGT(self, pos: ColVector) -> ColVector: + """ + @brief Perfect measurement from each landmark of the range and relative orientation of the robot + located at pos. + + @param pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + @return ColVector n x ([0] the range [1] the relative orientation of the robot), where + n is the number of landmarks. + """ + nbLandmarks = len(self.m_landmarks) + measurements = np.zeros(2*nbLandmarks) + for i in range(nbLandmarks): + landmarkMeas = self.m_landmarks[i].measureGT(pos) + measurements[2*i] = landmarkMeas[0] + measurements[(2*i) + 1] = landmarkMeas[1] + return ColVector(measurements) + + def measureWithNoise(self, pos: ColVector) -> ColVector: + """ + @brief Noisy measurement from each landmark of the range and relative orientation that + correspond to pos. + + @param[in] pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + @return ColVector n x ([0] the range [1] the relative orientation of the robot), where + n is the number of landmarks. + """ + nbLandmarks = len(self.m_landmarks) + measurements = np.zeros(2*nbLandmarks) + for i in range(nbLandmarks): + landmarkMeas = self.m_landmarks[i].measureWithNoise(pos) + measurements[2*i] = landmarkMeas[0] + measurements[(2*i) + 1] = landmarkMeas[1] + return ColVector(measurements) + +if __name__ == '__main__': + dt = 0.1 # Period of 0.1s + step = 1. # Number of update of the robot position between two UKF filtering + sigmaRange = 0.3 # Standard deviation of the range measurement: 0.3m + sigmaBearing = Math.rad(0.5) # Standard deviation of the bearing angle: 0.5deg + wheelbase = 0.5 # Wheelbase of 0.5m + processVariance = 0.0001 + landmarks = [ vpLandmarkMeasurements(5, 10, sigmaRange, sigmaBearing) + , vpLandmarkMeasurements(10, 5, sigmaRange, sigmaBearing) + , vpLandmarkMeasurements(15, 15, sigmaRange, sigmaBearing) + , vpLandmarkMeasurements(20, 5, sigmaRange, sigmaBearing) + , vpLandmarkMeasurements(0, 30, sigmaRange, sigmaBearing) + , vpLandmarkMeasurements(50, 30, sigmaRange, sigmaBearing) + , vpLandmarkMeasurements(40, 10, sigmaRange, sigmaBearing) + ] # Vector of landmarks constituing the grid + nbLandmarks = len(landmarks) # Number of landmarks constituing the grid + cmds = generateCommands() + nbCmds = len(cmds) + + # Creation of the simulated grid of landmarks and robot + grid = vpLandmarksGrid(landmarks) + 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) + + # The matrices require for the construction of the Unscented filter + P0 = Matrix([[0.1, 0., 0.], + [0., 0.1, 0.], + [0., 0., 0.05]]) # The initial estimate of the state covariance matrix + R1landmark = Matrix([[sigmaRange * sigmaRange, 0], [0, sigmaBearing*sigmaBearing]]) # The measurement covariance matrix for 1 landmark + 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) + Q = Q * processVariance + X0 = ColVector([2., 6., 0.3]) # robot_x, robot_y, robot_orientation(rad) + + # Creation of the Unscented Kalman filter + ukf = UnscentedKalman(Q, R, drawer, fx, grid.state_to_measurement) # The Unscented Kalman Filter instance + + # Initializing the state vector and state covariance matrix estimates + ukf.init(X0, P0) + ukf.setCommandStateFunction(robot.computeMotion) + ukf.setMeasurementMeanFunction(measurementMean) + ukf.setMeasurementResidualFunction(measurementResidual) + ukf.setStateAddFunction(add_state_vectors) + ukf.setStateMeanFunction(mean_state_vectors) + ukf.setStateResidualFunction(residual_state_vectors) + + # Initializing the Graphical User Interface if the needed libraries are available + if has_gui: + num_plots = 1 + plot = Plot(num_plots) + plot.initGraph(0, 2) + plot.setTitle(0, "Position of the robot") + plot.setUnitX(0, "Position along x(m)") + plot.setUnitY(0, "Position along y (m)") + plot.setLegend(0, 0, "GT") + plot.setLegend(0, 1, "Filtered") + + robot_pos = X0 + for i in range(nbCmds): + robot_pos = robot.move(cmds[i], robot_pos, dt / step) + + if (i % int(step) == 0): + # Perform the measurement + z = grid.measureWithNoise(robot_pos) + + # Use the UKF to filter the measurement + ukf.filter(z, dt, cmds[i]) + + if has_gui: + # Plot the filtered state + Xest = ukf.getXest() + plot.plot(0, 1, Xest[0], Xest[1]) + + # Update the GUI if available + if has_gui: + # Plot the ground truth + plot.plot(0, 0, robot_pos[0], robot_pos[1]) + + print('Finished') + input('Press enter to quit') From 5e39af007e681bf12750c97ad568d4ce45ed95ed Mon Sep 17 00:00:00 2001 From: rlagneau Date: Wed, 22 May 2024 07:54:01 +0200 Subject: [PATCH 03/16] Improved error message in inverseByCholesky function --- modules/core/src/math/matrix/vpMatrix_cholesky.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp index 30ae5de122..e4eee08196 100644 --- a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp +++ b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp @@ -193,8 +193,11 @@ vpMatrix vpMatrix::inverseByCholeskyLapack() const vpMatrix A = *this; dpotrf_((char *)"L", &rowNum_, A.data, &lda, &info); - if (info != 0) - throw(vpException(vpException::fatalError, "Cannot inverse by Cholesky with Lapack: error in dpotrf_()")); + if (info != 0) { + std::stringstream errMsg; + errMsg << "Cannot inverse by Cholesky with Lapack: error "<< info << " in dpotrf_()"; + throw(vpException(vpException::fatalError, errMsg.str())); + } dpotri_((char *)"L", &rowNum_, A.data, &lda, &info); if (info != 0) { From 451aa4c54f7e884b9d0f9cd4c81fd7fb7afdf659 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Wed, 22 May 2024 08:19:10 +0200 Subject: [PATCH 04/16] [EXAMPLE][PYTHON][FIX] Fixed the problem of Cholesky's inversion: it was due to numerical instability --- example/kalman/ukf-nonlinear-complex-example.cpp | 2 +- modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h | 2 +- modules/python/examples/ukf-nonlinear-complex-example.py | 3 +-- 3 files changed, 3 insertions(+), 4 deletions(-) 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) From 9e1dd1cfa565af58cf882f2ea79c751d41f882cb Mon Sep 17 00:00:00 2001 From: rlagneau Date: Wed, 22 May 2024 08:46:14 +0200 Subject: [PATCH 05/16] [PYTHON][DOC] Added ukf examples in Python documentation --- .../doc/rst/tutorials/misc/ukf-linear.rst | 15 +++++++ .../misc/ukf-nonlinear-linear-complex.rst | 13 +++++++ .../tutorials/misc/ukf-nonlinear-linear.rst | 13 +++++++ .../python/doc/rst/tutorials/tutorials.rst | 39 +++++++++++++++++++ 4 files changed, 80 insertions(+) create mode 100644 modules/python/doc/rst/tutorials/misc/ukf-linear.rst create mode 100644 modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear-complex.rst create mode 100644 modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear.rst diff --git a/modules/python/doc/rst/tutorials/misc/ukf-linear.rst b/modules/python/doc/rst/tutorials/misc/ukf-linear.rst new file mode 100644 index 0000000000..cf738155dc --- /dev/null +++ b/modules/python/doc/rst/tutorials/misc/ukf-linear.rst @@ -0,0 +1,15 @@ +Example on a linear case study on how to use Unscented Kalman filter +======================================================================= + +This example shows how to use an Unscented Kalman filter in Python. + +This example is a linear case study. It is not what is intended for the Unscented +Kalman filter, but it permits to get familiar with the different classes that +are used. + +The state vector and measurements are explained in the `C++ example `_. + + + +.. literalinclude:: /examples/ukf-linear-example.py + :language: python diff --git a/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear-complex.rst b/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear-complex.rst new file mode 100644 index 0000000000..a2396958cb --- /dev/null +++ b/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear-complex.rst @@ -0,0 +1,13 @@ +Example on a complex non-linear case study on how to use Unscented Kalman filter +======================================================================= + +This example shows how to use an Unscented Kalman filter in Python. + +This example is a complex non-linear case study. + +The state vector and measurements are explained in the `C++ example `_. + + + +.. literalinclude:: /examples/ukf-nonlinear-complex-example.py + :language: python diff --git a/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear.rst b/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear.rst new file mode 100644 index 0000000000..2b9bbd595b --- /dev/null +++ b/modules/python/doc/rst/tutorials/misc/ukf-nonlinear-linear.rst @@ -0,0 +1,13 @@ +Example on a non-linear case study on how to use Unscented Kalman filter +======================================================================= + +This example shows how to use an Unscented Kalman filter in Python. + +This example is a non-linear case study. + +The state vector and measurements are explained in the `C++ example `_. + + + +.. literalinclude:: /examples/ukf-nonlinear-example.py + :language: python diff --git a/modules/python/doc/rst/tutorials/tutorials.rst b/modules/python/doc/rst/tutorials/tutorials.rst index b166c9d656..e6b8b6cf26 100644 --- a/modules/python/doc/rst/tutorials/tutorials.rst +++ b/modules/python/doc/rst/tutorials/tutorials.rst @@ -23,3 +23,42 @@ Tracking :maxdepth: 2 tracking/* + + + +.. _Examples: + +Examples +==================== + + +Visual servoing +----------------------- + +.. toctree:: + :glob: + :maxdepth: 2 + + vs/* + + + +Tracking +----------------------- + +.. toctree:: + :glob: + :maxdepth: 2 + + tracking/* + + + +Other tools +----------------------- + +.. toctree:: + :glob: + :maxdepth: 2 + + misc/* From e885c41ad2a1fc1444560fc8e99b08dd403012d3 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Tue, 28 May 2024 16:03:28 +0200 Subject: [PATCH 06/16] [PYTHON][EXAMPLE] Adopted Python naming convention, fixed typo 'np.cosh/np.sinh=' and attributes naming --- .../python/doc/rst/tutorials/tutorials.rst | 28 -- .../examples/ukf-nonlinear-complex-example.py | 260 +++++++++--------- 2 files changed, 127 insertions(+), 161 deletions(-) diff --git a/modules/python/doc/rst/tutorials/tutorials.rst b/modules/python/doc/rst/tutorials/tutorials.rst index e6b8b6cf26..209b8747c6 100644 --- a/modules/python/doc/rst/tutorials/tutorials.rst +++ b/modules/python/doc/rst/tutorials/tutorials.rst @@ -26,34 +26,6 @@ Tracking -.. _Examples: - -Examples -==================== - - -Visual servoing ------------------------ - -.. toctree:: - :glob: - :maxdepth: 2 - - vs/* - - - -Tracking ------------------------ - -.. toctree:: - :glob: - :maxdepth: 2 - - tracking/* - - - Other tools ----------------------- diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index d5752b2d46..92080e808f 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -60,12 +60,12 @@ The mean of several angles must be computed in the Unscented Transform. The definition we chose to use is the following: - \f$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n \np.sin{\theta_i}}{n}, \frac{\sum_{i=1}^n \np.cos{\theta_i}}{n}) \f$ + \f$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n sin{\theta_i}}{n}, \frac{\sum_{i=1}^n cos{\theta_i}}{n}) \f$ As the Unscented Transform uses a weighted mean, the actual implementation of the weighted mean of several angles is the following: - \f$ mean_{weighted}(\boldsymbol{\theta}) = atan2 (\sum_{i=1}^n w_m^i \np.sin{\theta_i}, \sum_{i=1}^n w_m^i \np.cos{\theta_i}) \f$ + \f$ mean_{weighted}(\boldsymbol{\theta}) = atan2 (\sum_{i=1}^n w_m^i sin{\theta_i}, \sum_{i=1}^n w_m^i cos{\theta_i}) \f$ where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. @@ -87,7 +87,7 @@ has_gui = False import numpy as np -def normalizeAngle(angle: float) -> float: +def normalize_angle(angle: float) -> float: angleIn0to2pi = Math.modulo(angle, 2. * np.pi) angleInMinPiPi = angleIn0to2pi if angleInMinPiPi > np.pi: @@ -95,13 +95,13 @@ def normalizeAngle(angle: float) -> float: angleInMinPiPi = angleInMinPiPi - 2. * np.pi return angleInMinPiPi -def measurementMean(measurements: List[ColVector], wm: List[float]) -> ColVector: +def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVector: """ - @brief Compute the weighted mean of measurement vectors. + Compute the weighted mean of measurement vectors. - @param[in] measurements The measurement vectors, such as v[0] = dist_0 ; v[1] = bearing_0; + :param measurements The measurement vectors, such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... - @param[in] wm The associated weights. + :param wm The associated weights. @return ColVector The weighted mean. """ @@ -122,13 +122,13 @@ def measurementMean(measurements: List[ColVector], wm: List[float]) -> ColVector mean[(2*j)+1] = np.arctan2(sumSin[j], sumCos[j]) return ColVector(mean) -def measurementResidual(meas: ColVector, toSubstract: ColVector) -> ColVector: +def measurement_residual(meas: ColVector, toSubstract: ColVector) -> ColVector: """ - @brief Compute the substraction between two vectors expressed in the measurement space, + Compute the substraction between two vectors expressed in the measurement space, such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... - @param[in] meas Measurement to which we must substract something. - @param[in] toSubstract The something we must substract. + :param meas Measurement to which we must substract something. + :param toSubstract The something we must substract. @return ColVector \b meas - \b toSubstract . """ @@ -137,29 +137,29 @@ def measurementResidual(meas: ColVector, toSubstract: ColVector) -> ColVector: res = np.zeros(nbMeasures) for i in range(nbPoints): res[2*i] = meas[2*i] - toSubstract[2*i] - res[2*i+1] = normalizeAngle(meas[2*i + 1] - toSubstract[2*i + 1]) + res[2*i+1] = normalize_angle(meas[2*i + 1] - toSubstract[2*i + 1]) return ColVector(res) -def add_state_vectors(a: ColVector, b: ColVector) -> ColVector: +def state_add_vectors(a: ColVector, b: ColVector) -> ColVector: """ - @brief Compute the addition between two vectors expressed in the state space, + Compute the addition between two vectors expressed in the state space, such as v[0] = x ; v[1] = y; v[2] = heading . - @param a The first state vector to which another state vector must be added. - @param b The other state vector that must be added to a. + :param a The first state vector to which another state vector must be added. + :param b The other state vector that must be added to a. @return ColVector The sum a + b. """ add = a + b - return ColVector([add[0], add[1], normalizeAngle(add[2])] ) + return ColVector([add[0], add[1], normalize_angle(add[2])] ) -def mean_state_vectors(states: List[ColVector], wm: List[float]) -> ColVector: +def state_mean_vectors(states: List[ColVector], wm: List[float]) -> ColVector: """ - @brief Compute the weighted mean of state vectors. + Compute the weighted mean of state vectors. - @param states The state vectors. - @param wm The associated weights. + :param states The state vectors. + :param wm The associated weights. @return ColVector The weighted mean. """ mean = np.zeros(3) @@ -175,39 +175,39 @@ def mean_state_vectors(states: List[ColVector], wm: List[float]) -> ColVector: mean[2] = np.arctan2(sumSin, sumCos) return ColVector(mean) -def residual_state_vectors(a, b) -> ColVector: +def state_residual_vectors(a, b) -> ColVector: """ - @brief Compute the substraction between two vectors expressed in the state space, + Compute the substraction between two vectors expressed in the state space, such as v[0] = x ; v[1] = y; v[2] = heading . - @param a The first state vector to which another state vector must be substracted. - @param b The other state vector that must be substracted to a. + :param a The first state vector to which another state vector must be substracted. + :param b The other state vector that must be substracted to a. @return ColVector The substraction a - b. """ res = a - b - return ColVector([res[0], res[1], normalizeAngle(res[2])]) + return ColVector([res[0], res[1], normalize_angle(res[2])]) def fx(x: ColVector, dt: float) -> ColVector: """ - @brief As the state model {x, y, \f$ \theta \f$} does not contain any velocity + As the state model {x, y, \f$ \theta \f$} does not contain any velocity information, it does not evolve without commands. - @param x The internal state of the UKF. - @param dt The sampling time: how far in the future are we projecting x. + :param x The internal state of the UKF. + :param dt The sampling time: how far in the future are we projecting x. @return ColVector The updated internal state, projected in time, also known as the prior. """ return x -def generateTurnCommands(v: float, angleStart: float, angleStop: float, nbSteps: int) -> List[ColVector]: +def generate_turn_commands(v: float, angleStart: float, angleStop: float, nbSteps: int) -> List[ColVector]: """ - @brief Compute the commands realinp.sing a turn at constant linear velocity. + Compute the commands realinp.sing a turn at constant linear velocity. - @param v Constant linear velocity. - @param angleStart Starting angle (in degrees). - @param angleStop Stop angle (in degrees). - @param nbSteps Number of steps to perform the turn. + :param v Constant linear velocity. + :param angleStart Starting angle (in degrees). + :param angleStop Stop angle (in degrees). + :param nbSteps Number of steps to perform the turn. @return List[ColVector] The corresponding list of commands. """ cmds = [] @@ -218,9 +218,9 @@ def generateTurnCommands(v: float, angleStart: float, angleStop: float, nbSteps: cmds.append(cmd) return cmds -def generateCommands() -> List[ColVector]: +def generate_commands() -> List[ColVector]: """ - @brief Generate the list of commands for the simulation. + Generate the list of commands for the simulation. @return List[ColVector] The list of commands to use in the simulation """ @@ -234,27 +234,27 @@ def generateCommands() -> List[ColVector]: # Left turn lastLinearVelocity = cmds[len(cmds) -1][0] - leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 2, 15) + leftTurnCmds = generate_turn_commands(lastLinearVelocity, 0, 2, 15) cmds.extend(leftTurnCmds) for i in range(100): cmds.append(cmds[len(cmds) -1]) # Right turn lastLinearVelocity = cmds[len(cmds) -1][0] - rightTurnCmds = generateTurnCommands(lastLinearVelocity, 2, -2, 15); + rightTurnCmds = generate_turn_commands(lastLinearVelocity, 2, -2, 15); cmds.extend(rightTurnCmds) for i in range(200): cmds.append(cmds[len(cmds) -1]) # Left turn again lastLinearVelocity = cmds[len(cmds) -1][0] - leftTurnCmds = generateTurnCommands(lastLinearVelocity, -2, 0, 15) + leftTurnCmds = generate_turn_commands(lastLinearVelocity, -2, 0, 15) cmds.extend(leftTurnCmds) for i in range(150): cmds.append(cmds[len(cmds) -1]) lastLinearVelocity = cmds[len(cmds) -1][0] - leftTurnCmds = generateTurnCommands(lastLinearVelocity, 0, 1, 25) + leftTurnCmds = generate_turn_commands(lastLinearVelocity, 0, 1, 25) cmds.extend(leftTurnCmds) for i in range(150): cmds.append(cmds[len(cmds) -1]) @@ -263,23 +263,23 @@ def generateCommands() -> List[ColVector]: class vpBicycleModel: """ - @brief Class that approximates a 4-wheel robot unp.sing a bicycle model. + Class that approximates a 4-wheel robot unp.sing a bicycle model. """ def __init__(self, w: float): """ - @brief Construct a new vpBicycleModel object. + Construct a new vpBicycleModel object. - @param w The length of the wheelbase. + :param w The length of the wheelbase. """ - self.m_w = w # The length of the wheelbase. + self._w = w # The length of the wheelbase. - def computeMotion(self, u: ColVector, x: ColVector, dt: float) -> ColVector: + def compute_motion(self, u: ColVector, x: ColVector, dt: float) -> ColVector: """ - @brief Models the effect of the command on the state model. + Models the effect of the command on the state model. - @param u The commands. u[0] = velocity ; u[1] = steeringAngle . - @param x The state model. x[0] = x ; x[1] = y ; x[2] = heading - @param dt The period. + :param u The commands. u[0] = velocity ; u[1] = steeringAngle . + :param x The state model. x[0] = x ; x[1] = y ; x[2] = heading + :param dt The period. @return ColVector The state model after applying the command. """ heading = x[2] @@ -289,15 +289,15 @@ def computeMotion(self, u: ColVector, x: ColVector, dt: float) -> ColVector: if (abs(steeringAngle) > 0.001): # The robot is turning - beta = (distance / self.m_w) * np.tan(steeringAngle) - radius = self.m_w / np.tan(steeringAngle); - np.sinh = np.sin(heading) - np.sinhb = np.sin(heading + beta) - np.cosh = np.cos(heading) - np.coshb = np.cos(heading + beta) + beta = (distance / self._w) * np.tan(steeringAngle) + radius = self._w / np.tan(steeringAngle); + sinh = np.sin(heading) + sinhb = np.sin(heading + beta) + cosh = np.cos(heading) + coshb = np.cos(heading + beta) motion = ColVector([ - -radius * np.sinh + radius * np.sinhb, - radius * np.cosh - radius * np.coshb, + -radius * sinh + radius * sinhb, + radius * cosh - radius * coshb, beta ]) return motion @@ -312,138 +312,138 @@ def computeMotion(self, u: ColVector, x: ColVector, dt: float) -> ColVector: def move(self, u: ColVector, x: ColVector, dt: float) -> ColVector: """ - @brief Models the effect of the command on the state model. + Models the effect of the command on the state model. - @param u The commands. u[0] = velocity ; u[1] = steeringAngle . - @param x The state model. x[0] = x ; x[1] = y ; x[2] = heading - @param dt The period. + :param u The commands. u[0] = velocity ; u[1] = steeringAngle . + :param x The state model. x[0] = x ; x[1] = y ; x[2] = heading + :param dt The period. @return ColVector The state model after applying the command. """ - motion = self.computeMotion(u, x, dt) + motion = self.compute_motion(u, x, dt) newX = x + motion - normalizedAngle = normalizeAngle(newX[2]) + normalizedAngle = normalize_angle(newX[2]) return ColVector([newX[0], newX[1], normalizedAngle]) -class vpLandmarkMeasurements: +class LandmarkMeasurements: """ - @brief Class that permits to convert the position + heading of the 4-wheel + Class that permits to convert the position + heading of the 4-wheel robot into measurements from a landmark. """ def __init__(self, x: float, y: float, range_std: float, rel_angle_std: float): """ - @brief Construct a new vpLandmarkMeasurements object. + Construct a new LandmarkMeasurements object. - @param x The position along the x-axis of the landmark. - @param y The position along the y-axis of the landmark. - @param range_std The standard deviation of the range measurements. - @param rel_angle_std The standard deviation of the relative angle measurements. + :param x The position along the x-axis of the landmark. + :param y The position along the y-axis of the landmark. + :param range_std The standard deviation of the range measurements. + :param rel_angle_std The standard deviation of the relative angle measurements. """ - self.m_x = x # The position along the x-axis of the landmark - self.m_y = y # The position along the y-axis of the landmark - self.m_range_std = range_std # The standard deviation of the range measurement - self.m_rel_angle_std = rel_angle_std # The standard deviation of the relative angle measurement + self._x = x # The position along the x-axis of the landmark + self._y = y # The position along the y-axis of the landmark + self._range_std = range_std # The standard deviation of the range measurement + self._rel_angle_std = rel_angle_std # The standard deviation of the relative angle measurement np.random.seed(4224) def state_to_measurement(self, chi: ColVector) -> ColVector: """ - @brief Convert the prior of the UKF into the measurement space. + Convert the prior of the UKF into the measurement space. - @param chi The prior. + :param chi The prior. @return ColVector The prior expressed in the measurement space. """ - dx = self.m_x - chi[0] - dy = self.m_y - chi[1] - meas = ColVector([sqrt(dx * dx + dy * dy), normalizeAngle(np.arctan2(dy, dx) - chi[2])]) + dx = self._x - chi[0] + dy = self._y - chi[1] + meas = ColVector([sqrt(dx * dx + dy * dy), normalize_angle(np.arctan2(dy, dx) - chi[2])]) return meas - def measureGT(self, pos: ColVector) -> ColVector: + def measure_gt(self, pos: ColVector) -> ColVector: """ - @brief Perfect measurement of the range and relative orientation of the robot + Perfect measurement of the range and relative orientation of the robot located at pos. - @param pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + :param pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. @return ColVector [0] the range [1] the relative orientation of the robot. """ - dx = self.m_x - pos[0] - dy = self.m_y - pos[1] + dx = self._x - pos[0] + dy = self._y - pos[1] range = sqrt(dx * dx + dy * dy) - orientation = normalizeAngle(np.arctan2(dy, dx) - pos[2]) + orientation = normalize_angle(np.arctan2(dy, dx) - pos[2]) measurements = ColVector([range, orientation]) return measurements - def measureWithNoise(self, pos: ColVector) -> ColVector: + def measure_with_noise(self, pos: ColVector) -> ColVector: """ - @brief Noisy measurement of the range and relative orientation that + Noisy measurement of the range and relative orientation that correspond to pos. - @param pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + :param pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). @return ColVector [0] the range [1] the relative orientation. """ - measurementsGT = self.measureGT(pos) + measurementsGT = self.measure_gt(pos) measurementsNoisy = measurementsGT - range = measurementsNoisy[0] + np.random.normal(0., self.m_range_std) - relAngle = normalizeAngle(measurementsNoisy[1] + np.random.normal(0., self.m_rel_angle_std)) + range = measurementsNoisy[0] + np.random.normal(0., self._range_std) + relAngle = normalize_angle(measurementsNoisy[1] + np.random.normal(0., self._rel_angle_std)) return ColVector([range, relAngle]) -class vpLandmarksGrid: +class LandmarksGrid: """ - @brief Class that represent a grid of landmarks that measure the distance and + Class that represent a grid of landmarks that measure the distance and relative orientation of the 4-wheel robot. """ - def __init__(self, landmarks: List[vpLandmarkMeasurements]): + def __init__(self, landmarks: List[LandmarkMeasurements]): """ - @brief Construct a new vpLandmarksGrid object. + Construct a new LandmarksGrid object. - @param landmarks The list of landmarks forming the grid. + :param landmarks The list of landmarks forming the grid. """ - self.m_landmarks = landmarks # The list of landmarks forming the grid. + self._landmarks = landmarks # The list of landmarks forming the grid. def state_to_measurement(self, chi: ColVector) -> ColVector: """ - @brief Convert the prior of the UKF into the measurement space. + Convert the prior of the UKF into the measurement space. - @param chi The prior. + :param chi The prior. @return ColVector The prior expressed in the measurement space. """ - nbLandmarks = len(self.m_landmarks) + nbLandmarks = len(self._landmarks) measurements = np.zeros(2*nbLandmarks) for i in range (nbLandmarks): - landmarkMeas = self.m_landmarks[i].state_to_measurement(chi) + landmarkMeas = self._landmarks[i].state_to_measurement(chi) measurements[2*i] = landmarkMeas[0] measurements[(2*i) + 1] = landmarkMeas[1] return ColVector(measurements) - def measureGT(self, pos: ColVector) -> ColVector: + def measure_gt(self, pos: ColVector) -> ColVector: """ - @brief Perfect measurement from each landmark of the range and relative orientation of the robot + Perfect measurement from each landmark of the range and relative orientation of the robot located at pos. - @param pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + :param pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. @return ColVector n x ([0] the range [1] the relative orientation of the robot), where n is the number of landmarks. """ - nbLandmarks = len(self.m_landmarks) + nbLandmarks = len(self._landmarks) measurements = np.zeros(2*nbLandmarks) for i in range(nbLandmarks): - landmarkMeas = self.m_landmarks[i].measureGT(pos) + landmarkMeas = self._landmarks[i].measure_gt(pos) measurements[2*i] = landmarkMeas[0] measurements[(2*i) + 1] = landmarkMeas[1] return ColVector(measurements) - def measureWithNoise(self, pos: ColVector) -> ColVector: + def measure_with_noise(self, pos: ColVector) -> ColVector: """ - @brief Noisy measurement from each landmark of the range and relative orientation that + Noisy measurement from each landmark of the range and relative orientation that correspond to pos. - @param[in] pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + :param pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). @return ColVector n x ([0] the range [1] the relative orientation of the robot), where n is the number of landmarks. """ - nbLandmarks = len(self.m_landmarks) + nbLandmarks = len(self._landmarks) measurements = np.zeros(2*nbLandmarks) for i in range(nbLandmarks): - landmarkMeas = self.m_landmarks[i].measureWithNoise(pos) + landmarkMeas = self._landmarks[i].measure_with_noise(pos) measurements[2*i] = landmarkMeas[0] measurements[(2*i) + 1] = landmarkMeas[1] return ColVector(measurements) @@ -451,34 +451,28 @@ def measureWithNoise(self, pos: ColVector) -> ColVector: if __name__ == '__main__': dt = 0.1 # Period of 0.1s step = 1. # Number of update of the robot position between two UKF filtering - sigmaRange = 0.3 # Standard deviation of the range measurement: 0.3m - sigmaBearing = Math.rad(0.5) # Standard deviation of the bearing angle: 0.5deg + sigma_range = 0.3 # Standard deviation of the range measurement: 0.3m + sigma_bearing = Math.rad(0.5) # Standard deviation of the bearing angle: 0.5deg wheelbase = 0.5 # Wheelbase of 0.5m processVariance = 0.0001 - landmarks = [ vpLandmarkMeasurements(5, 10, sigmaRange, sigmaBearing) - , vpLandmarkMeasurements(10, 5, sigmaRange, sigmaBearing) - , vpLandmarkMeasurements(15, 15, sigmaRange, sigmaBearing) - , vpLandmarkMeasurements(20, 5, sigmaRange, sigmaBearing) - , vpLandmarkMeasurements(0, 30, sigmaRange, sigmaBearing) - , vpLandmarkMeasurements(50, 30, sigmaRange, sigmaBearing) - , vpLandmarkMeasurements(40, 10, sigmaRange, sigmaBearing) - ] # Vector of landmarks constituing the grid + positions = [ (5, 10) , (10, 5), (15, 15), (20, 5), (0, 30), (50, 30), (40, 10)] # Positions of the landmarks constituing the grid + landmarks = [LandmarkMeasurements(x, y, sigma_range, sigma_bearing) for x,y in positions] # Vector of landmarks constituing the grid nbLandmarks = len(landmarks) # Number of landmarks constituing the grid - cmds = generateCommands() + cmds = generate_commands() nbCmds = len(cmds) # Creation of the simulated grid of landmarks and robot - grid = vpLandmarksGrid(landmarks) + grid = LandmarksGrid(landmarks) robot = vpBicycleModel(wheelbase) # The object that draws the sigma points used by the UKF - drawer = UKSigmaDrawerMerwe(n=3, alpha=0.1, beta=2, kappa=0, resFunc=residual_state_vectors, addFunc=add_state_vectors) + drawer = UKSigmaDrawerMerwe(n=3, alpha=0.1, beta=2, kappa=0, resFunc=state_residual_vectors, addFunc=state_add_vectors) # The matrices require for the construction of the Unscented filter P0 = Matrix([[0.1, 0., 0.], [0., 0.1, 0.], [0., 0., 0.05]]) # The initial estimate of the state covariance matrix - R1landmark = Matrix([[sigmaRange * sigmaRange, 0], [0, sigmaBearing*sigmaBearing]]) # The measurement covariance matrix for 1 landmark + R1landmark = Matrix([[sigma_range * sigma_range, 0], [0, sigma_bearing*sigma_bearing]]) # The measurement covariance matrix for 1 landmark 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) @@ -493,12 +487,12 @@ def measureWithNoise(self, pos: ColVector) -> ColVector: # Initializing the state vector and state covariance matrix estimates ukf.init(X0, P0) - ukf.setCommandStateFunction(robot.computeMotion) - ukf.setMeasurementMeanFunction(measurementMean) - ukf.setMeasurementResidualFunction(measurementResidual) - ukf.setStateAddFunction(add_state_vectors) - ukf.setStateMeanFunction(mean_state_vectors) - ukf.setStateResidualFunction(residual_state_vectors) + ukf.setCommandStateFunction(robot.compute_motion) + ukf.setMeasurementMeanFunction(measurement_mean) + ukf.setMeasurementResidualFunction(measurement_residual) + ukf.setStateAddFunction(state_add_vectors) + ukf.setStateMeanFunction(state_mean_vectors) + ukf.setStateResidualFunction(state_residual_vectors) # Initializing the Graphical User Interface if the needed libraries are available if has_gui: @@ -517,7 +511,7 @@ def measureWithNoise(self, pos: ColVector) -> ColVector: if (i % int(step) == 0): # Perform the measurement - z = grid.measureWithNoise(robot_pos) + z = grid.measure_with_noise(robot_pos) # Use the UKF to filter the measurement ukf.filter(z, dt, cmds[i]) From d1140dc7036446016a92cadc5456012c7dc8dee1 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Tue, 28 May 2024 16:07:52 +0200 Subject: [PATCH 07/16] [PYTHON][EXAMPLE] Corrected typo --- modules/python/examples/ukf-nonlinear-complex-example.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index 92080e808f..bdb5464719 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -225,7 +225,7 @@ def generate_commands() -> List[ColVector]: @return List[ColVector] The list of commands to use in the simulation """ cmds = [] - # Starting by an straight line acceleration + # Starting by a straight line acceleration nbSteps = 30 dv = (1.1 - 0.001) / float(nbSteps - 1) for i in range(nbSteps): @@ -290,7 +290,7 @@ def compute_motion(self, u: ColVector, x: ColVector, dt: float) -> ColVector: if (abs(steeringAngle) > 0.001): # The robot is turning beta = (distance / self._w) * np.tan(steeringAngle) - radius = self._w / np.tan(steeringAngle); + radius = self._w / np.tan(steeringAngle) sinh = np.sin(heading) sinhb = np.sin(heading + beta) cosh = np.cos(heading) From 1f05e0918ad24cb5e450a9c11d8569b2c67e7019 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Wed, 29 May 2024 13:57:39 +0200 Subject: [PATCH 08/16] [PYTHON][EXAMPLE] Vectorized computation of mean and residual --- .../python/examples/ukf-nonlinear-complex-example.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index bdb5464719..2a81d71809 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -118,8 +118,8 @@ def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVecto sumCos[j] += np.cos(measurements[i][(2*j)+1]) * wm[i] sumSin[j] += np.sin(measurements[i][(2*j)+1]) * wm[i] - for j in range(nbLandmarks): - mean[(2*j)+1] = np.arctan2(sumSin[j], sumCos[j]) + orientations = np.arctan2(sumSin, sumCos) + mean[1::2] = orientations[0::1] return ColVector(mean) def measurement_residual(meas: ColVector, toSubstract: ColVector) -> ColVector: @@ -132,12 +132,8 @@ def measurement_residual(meas: ColVector, toSubstract: ColVector) -> ColVector: @return ColVector \b meas - \b toSubstract . """ - nbMeasures = meas.size() - nbPoints = int(nbMeasures / 2) - res = np.zeros(nbMeasures) - for i in range(nbPoints): - res[2*i] = meas[2*i] - toSubstract[2*i] - res[2*i+1] = normalize_angle(meas[2*i + 1] - toSubstract[2*i + 1]) + res = meas.numpy() - toSubstract.numpy() + res[1::2] = [normalize_angle(angle) for angle in res[1::2]] return ColVector(res) def state_add_vectors(a: ColVector, b: ColVector) -> ColVector: From 4d80dfbedfdd05f11c0331ed8a064dc7fa485cc7 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Thu, 30 May 2024 10:04:34 +0200 Subject: [PATCH 09/16] [PYTHON][EXAMPLE][CLEAN] Followed naming convention + documentation rules of Python --- modules/python/examples/ukf-linear-example.py | 48 ++-- .../examples/ukf-nonlinear-complex-example.py | 150 ++++++------- .../python/examples/ukf-nonlinear-example.py | 207 +++++++++--------- 3 files changed, 202 insertions(+), 203 deletions(-) diff --git a/modules/python/examples/ukf-linear-example.py b/modules/python/examples/ukf-linear-example.py index 1121491e8f..d0ba4b2db4 100644 --- a/modules/python/examples/ukf-linear-example.py +++ b/modules/python/examples/ukf-linear-example.py @@ -70,12 +70,12 @@ def fx(x: ColVector, dt: float) -> ColVector: """ - @brief Process function that projects in time the internal state of the UKF. + Process function that projects in time the internal state of the UKF. - @param x The internal state of the UKF. - @param dt The sampling time: how far in the future are we projecting x. + :param x: The internal state of the UKF. + :param dt: The sampling time: how far in the future are we projecting x. - @return ColVector The updated internal state, projected in time, also known as the prior. + :return ColVector: The updated internal state, projected in time, also known as the prior. """ return ColVector([ x[0] + dt * x[1], @@ -87,11 +87,11 @@ def fx(x: ColVector, dt: float) -> ColVector: def hx(x: ColVector) -> ColVector: """ - @brief Measurement function that expresses the internal state of the UKF in the measurement space. + Measurement function that expresses the internal state of the UKF in the measurement space. - @param x The internal state of the UKF. + :param x: The internal state of the UKF. - @return ColVector The internal state, expressed in the measurement space. + :return ColVector: The internal state, expressed in the measurement space. """ return ColVector([ x[0], @@ -100,34 +100,34 @@ def hx(x: ColVector) -> ColVector: def add_state_vectors(a, b) -> ColVector: """ - @brief Method that permits to add two state vectors. + Method that permits to add two state vectors. - @param a The first state vector to which another state vector must be added. - @param b The other state vector that must be added to a. + :param a: The first state vector to which another state vector must be added. + :param b: The other state vector that must be added to a. - @return ColVector The sum a + b. + :return ColVector: The sum a + b. """ return a + b def residual_state_vectors(a, b) -> ColVector: """ - @brief Method that permits to substract a state vector to another. + Method that permits to substract a state vector to another. - @param a The first state vector to which another state vector must be substracted. - @param b The other state vector that must be substracted to a. + :param a: The first state vector to which another state vector must be substracted. + :param b: The other state vector that must be substracted to a. - @return ColVector The substraction a - b. + :return ColVector: The substraction a - b. """ return a - b def generate_Q_matrix(dt: float) -> Matrix: """ - @brief Method that generates the process covariance matrix for a process for which the + Method that generates the process covariance matrix for a process for which the state vector can be written as (x, dx/dt)^T - @param dt The sampling period. + :param dt: The sampling period. - @return Matrix The corresponding process covariance matrix. + :return Matrix: The corresponding process covariance matrix. """ return Matrix( [[dt**3/3, dt**2/2, 0, 0], @@ -142,16 +142,16 @@ def generate_Q_matrix(dt: float) -> Matrix: gt_dX = ColVector([gt_dx, gt_dy]) # The displacement vector between two timesteps gt_vx = gt_dx / dt # The velocity along the x-axis gt_vy = gt_dy / dt # The velocity along the y-axis - procVar = 0.000004 # The variance of the process function - sigmaXmeas = 0.05 # The standard deviation of the measurement noise for the x-axis measurement - sigmaYmeas = 0.05 # The standard deviation of the measurement noise for the y-axis measurement + proc_var = 0.000004 # The variance of the process function + sigma_x_meas = 0.05 # The standard deviation of the measurement noise for the x-axis measurement + sigma_y_meas = 0.05 # The standard deviation of the measurement noise for the y-axis measurement # The object that draws the sigma points used by the UKF drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=residual_state_vectors, addFunc=add_state_vectors) P0 = Matrix(np.eye(4) * 1.) # The initial estimate of the state covariance matrix R = Matrix(np.eye(2) * 0.01) # The measurement covariance matrix - Q = generate_Q_matrix(dt) * procVar # The process covariance matrix + Q = generate_Q_matrix(dt) * proc_var # The process covariance matrix ukf = UnscentedKalman(Q, R, drawer, fx, hx) # The Unscented Kalman Filter instance # Initializing the state vector and state covariance matrix estimates @@ -186,8 +186,8 @@ def generate_Q_matrix(dt: float) -> Matrix: for i in range(100): # Creating noisy measurements - x_meas = gt_X[0] + np.random.normal(0.0, sigmaXmeas) - y_meas = gt_X[1] + np.random.normal(0.0, sigmaYmeas) + x_meas = gt_X[0] + np.random.normal(0.0, sigma_x_meas) + y_meas = gt_X[1] + np.random.normal(0.0, sigma_y_meas) z = ColVector([x_meas, y_meas]) # Filtering using the UKF diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index 2a81d71809..de4e03a93e 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -88,51 +88,51 @@ import numpy as np def normalize_angle(angle: float) -> float: - angleIn0to2pi = Math.modulo(angle, 2. * np.pi) - angleInMinPiPi = angleIn0to2pi - if angleInMinPiPi > np.pi: + angle_0_to_2pi = Math.modulo(angle, 2. * np.pi) + angle_MinPi_Pi = angle_0_to_2pi + if angle_MinPi_Pi > np.pi: # Substract 2 PI to be in interval [-Pi; Pi] - angleInMinPiPi = angleInMinPiPi - 2. * np.pi - return angleInMinPiPi + angle_MinPi_Pi = angle_MinPi_Pi - 2. * np.pi + return angle_MinPi_Pi def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVector: """ Compute the weighted mean of measurement vectors. - :param measurements The measurement vectors, such as v[0] = dist_0 ; v[1] = bearing_0; + :param measurements: The measurement vectors, such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... - :param wm The associated weights. + :param wm: The associated weights. - @return ColVector The weighted mean. + :return ColVector: The weighted mean. """ - nbPoints = len(measurements) - sizeMeasurement = measurements[0].size() - nbLandmarks = sizeMeasurement // 2 - mean = np.zeros(sizeMeasurement) - sumCos = np.zeros(nbLandmarks) - sumSin = np.zeros(nbLandmarks) - - for i in range(nbPoints): - for j in range(nbLandmarks): + nb_points = len(measurements) + size_measurement = measurements[0].size() + nb_landmarks = size_measurement // 2 + mean = np.zeros(size_measurement) + sum_cos = np.zeros(nb_landmarks) + sum_sin = np.zeros(nb_landmarks) + + for i in range(nb_points): + for j in range(nb_landmarks): mean[2*j] += wm[i] * measurements[i][2*j] - sumCos[j] += np.cos(measurements[i][(2*j)+1]) * wm[i] - sumSin[j] += np.sin(measurements[i][(2*j)+1]) * wm[i] + sum_cos[j] += np.cos(measurements[i][(2*j)+1]) * wm[i] + sum_sin[j] += np.sin(measurements[i][(2*j)+1]) * wm[i] - orientations = np.arctan2(sumSin, sumCos) - mean[1::2] = orientations[0::1] + orientations = np.arctan2(sum_sin, sum_cos) + mean[1::2] = orientations return ColVector(mean) -def measurement_residual(meas: ColVector, toSubstract: ColVector) -> ColVector: +def measurement_residual(meas: ColVector, to_substract: ColVector) -> ColVector: """ Compute the substraction between two vectors expressed in the measurement space, such as v[0] = dist_0 ; v[1] = bearing_0; v[2] = dist_1 ; v[3] = bearing_1 ... - :param meas Measurement to which we must substract something. - :param toSubstract The something we must substract. + :param meas: Measurement to which we must substract something. + :param toSubstract: The something we must substract. - @return ColVector \b meas - \b toSubstract . + :return ColVector: \b meas - \b toSubstract . """ - res = meas.numpy() - toSubstract.numpy() + res = meas.numpy() - to_substract.numpy() res[1::2] = [normalize_angle(angle) for angle in res[1::2]] return ColVector(res) @@ -141,10 +141,10 @@ def state_add_vectors(a: ColVector, b: ColVector) -> ColVector: Compute the addition between two vectors expressed in the state space, such as v[0] = x ; v[1] = y; v[2] = heading . - :param a The first state vector to which another state vector must be added. - :param b The other state vector that must be added to a. + :param a: The first state vector to which another state vector must be added. + :param b: The other state vector that must be added to a. - @return ColVector The sum a + b. + :return ColVector: The sum a + b. """ add = a + b return ColVector([add[0], add[1], normalize_angle(add[2])] ) @@ -154,9 +154,9 @@ def state_mean_vectors(states: List[ColVector], wm: List[float]) -> ColVector: """ Compute the weighted mean of state vectors. - :param states The state vectors. - :param wm The associated weights. - @return ColVector The weighted mean. + :param states: The state vectors. + :param wm: The associated weights. + :return ColVector: The weighted mean. """ mean = np.zeros(3) nbPoints = len(states) @@ -176,10 +176,10 @@ def state_residual_vectors(a, b) -> ColVector: Compute the substraction between two vectors expressed in the state space, such as v[0] = x ; v[1] = y; v[2] = heading . - :param a The first state vector to which another state vector must be substracted. - :param b The other state vector that must be substracted to a. + :param a: The first state vector to which another state vector must be substracted. + :param b: The other state vector that must be substracted to a. - @return ColVector The substraction a - b. + :return ColVector: The substraction a - b. """ res = a - b return ColVector([res[0], res[1], normalize_angle(res[2])]) @@ -189,22 +189,22 @@ def fx(x: ColVector, dt: float) -> ColVector: As the state model {x, y, \f$ \theta \f$} does not contain any velocity information, it does not evolve without commands. - :param x The internal state of the UKF. - :param dt The sampling time: how far in the future are we projecting x. + :param x: The internal state of the UKF. + :param dt: The sampling time: how far in the future are we projecting x. - @return ColVector The updated internal state, projected in time, also known as the prior. + :return ColVector: The updated internal state, projected in time, also known as the prior. """ return x def generate_turn_commands(v: float, angleStart: float, angleStop: float, nbSteps: int) -> List[ColVector]: """ - Compute the commands realinp.sing a turn at constant linear velocity. + Compute the commands realising a turn at constant linear velocity. - :param v Constant linear velocity. - :param angleStart Starting angle (in degrees). - :param angleStop Stop angle (in degrees). - :param nbSteps Number of steps to perform the turn. - @return List[ColVector] The corresponding list of commands. + :param v: Constant linear velocity. + :param angleStart: Starting angle (in degrees). + :param angleStop: Stop angle (in degrees). + :param nbSteps: Number of steps to perform the turn. + :return List[ColVector]: The corresponding list of commands. """ cmds = [] dTheta = Math.rad(angleStop - angleStart) / float(nbSteps - 1); @@ -218,7 +218,7 @@ def generate_commands() -> List[ColVector]: """ Generate the list of commands for the simulation. - @return List[ColVector] The list of commands to use in the simulation + :return List[ColVector]: The list of commands to use in the simulation """ cmds = [] # Starting by a straight line acceleration @@ -265,7 +265,7 @@ def __init__(self, w: float): """ Construct a new vpBicycleModel object. - :param w The length of the wheelbase. + :param w:The length of the wheelbase. """ self._w = w # The length of the wheelbase. @@ -273,10 +273,10 @@ def compute_motion(self, u: ColVector, x: ColVector, dt: float) -> ColVector: """ Models the effect of the command on the state model. - :param u The commands. u[0] = velocity ; u[1] = steeringAngle . - :param x The state model. x[0] = x ; x[1] = y ; x[2] = heading - :param dt The period. - @return ColVector The state model after applying the command. + :param u: The commands. u[0] = velocity ; u[1] = steeringAngle . + :param x: The state model. x[0] = x ; x[1] = y ; x[2] = heading + :param dt: The period. + :return ColVector: The state model after applying the command. """ heading = x[2] vel = u[0] @@ -310,10 +310,10 @@ def move(self, u: ColVector, x: ColVector, dt: float) -> ColVector: """ Models the effect of the command on the state model. - :param u The commands. u[0] = velocity ; u[1] = steeringAngle . - :param x The state model. x[0] = x ; x[1] = y ; x[2] = heading - :param dt The period. - @return ColVector The state model after applying the command. + :param u: The commands. u[0] = velocity ; u[1] = steeringAngle . + :param x: The state model. x[0] = x ; x[1] = y ; x[2] = heading + :param dt: The period. + :return ColVector: The state model after applying the command. """ motion = self.compute_motion(u, x, dt) newX = x + motion @@ -329,10 +329,10 @@ def __init__(self, x: float, y: float, range_std: float, rel_angle_std: float): """ Construct a new LandmarkMeasurements object. - :param x The position along the x-axis of the landmark. - :param y The position along the y-axis of the landmark. - :param range_std The standard deviation of the range measurements. - :param rel_angle_std The standard deviation of the relative angle measurements. + :param x: The position along the x-axis of the landmark. + :param y: The position along the y-axis of the landmark. + :param range_std: The standard deviation of the range measurements. + :param rel_angle_std: The standard deviation of the relative angle measurements. """ self._x = x # The position along the x-axis of the landmark self._y = y # The position along the y-axis of the landmark @@ -344,8 +344,8 @@ def state_to_measurement(self, chi: ColVector) -> ColVector: """ Convert the prior of the UKF into the measurement space. - :param chi The prior. - @return ColVector The prior expressed in the measurement space. + :param chi: The prior. + :return ColVector: The prior expressed in the measurement space. """ dx = self._x - chi[0] dy = self._y - chi[1] @@ -357,8 +357,8 @@ def measure_gt(self, pos: ColVector) -> ColVector: Perfect measurement of the range and relative orientation of the robot located at pos. - :param pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. - @return ColVector [0] the range [1] the relative orientation of the robot. + :param pos: The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + :return ColVector: [0] the range [1] the relative orientation of the robot. """ dx = self._x - pos[0] dy = self._y - pos[1] @@ -372,8 +372,8 @@ def measure_with_noise(self, pos: ColVector) -> ColVector: Noisy measurement of the range and relative orientation that correspond to pos. - :param pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). - @return ColVector [0] the range [1] the relative orientation. + :param pos: The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + :return ColVector: [0] the range [1] the relative orientation. """ measurementsGT = self.measure_gt(pos) measurementsNoisy = measurementsGT @@ -391,7 +391,7 @@ def __init__(self, landmarks: List[LandmarkMeasurements]): """ Construct a new LandmarksGrid object. - :param landmarks The list of landmarks forming the grid. + :param landmarks: The list of landmarks forming the grid. """ self._landmarks = landmarks # The list of landmarks forming the grid. @@ -399,8 +399,8 @@ def state_to_measurement(self, chi: ColVector) -> ColVector: """ Convert the prior of the UKF into the measurement space. - :param chi The prior. - @return ColVector The prior expressed in the measurement space. + :param chi: The prior. + :return ColVector: The prior expressed in the measurement space. """ nbLandmarks = len(self._landmarks) measurements = np.zeros(2*nbLandmarks) @@ -415,8 +415,8 @@ def measure_gt(self, pos: ColVector) -> ColVector: Perfect measurement from each landmark of the range and relative orientation of the robot located at pos. - :param pos The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. - @return ColVector n x ([0] the range [1] the relative orientation of the robot), where + :param pos: The actual position of the robot (pos[0]: x, pos[1]: y, pos[2] = heading. + :return ColVector: n x ([0] the range [1] the relative orientation of the robot), where n is the number of landmarks. """ nbLandmarks = len(self._landmarks) @@ -432,8 +432,8 @@ def measure_with_noise(self, pos: ColVector) -> ColVector: Noisy measurement from each landmark of the range and relative orientation that correspond to pos. - :param pos The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). - @return ColVector n x ([0] the range [1] the relative orientation of the robot), where + :param pos: The actual position of the robot (pos[0]: x ; pos[1] = y ; pos[2] = heading). + :return ColVector: n x ([0] the range [1] the relative orientation of the robot), where n is the number of landmarks. """ nbLandmarks = len(self._landmarks) @@ -450,12 +450,12 @@ def measure_with_noise(self, pos: ColVector) -> ColVector: sigma_range = 0.3 # Standard deviation of the range measurement: 0.3m sigma_bearing = Math.rad(0.5) # Standard deviation of the bearing angle: 0.5deg wheelbase = 0.5 # Wheelbase of 0.5m - processVariance = 0.0001 + process_variance = 0.0001 positions = [ (5, 10) , (10, 5), (15, 15), (20, 5), (0, 30), (50, 30), (40, 10)] # Positions of the landmarks constituing the grid landmarks = [LandmarkMeasurements(x, y, sigma_range, sigma_bearing) for x,y in positions] # Vector of landmarks constituing the grid nbLandmarks = len(landmarks) # Number of landmarks constituing the grid cmds = generate_commands() - nbCmds = len(cmds) + nb_cmds = len(cmds) # Creation of the simulated grid of landmarks and robot grid = LandmarksGrid(landmarks) @@ -475,7 +475,7 @@ def measure_with_noise(self, pos: ColVector) -> ColVector: Q = Matrix() # The process covariance matrix Q.eye(3) - Q = Q * processVariance + Q = Q * process_variance X0 = ColVector([2., 6., 0.3]) # robot_x, robot_y, robot_orientation(rad) # Creation of the Unscented Kalman filter @@ -502,7 +502,7 @@ def measure_with_noise(self, pos: ColVector) -> ColVector: plot.setLegend(0, 1, "Filtered") robot_pos = X0 - for i in range(nbCmds): + for i in range(nb_cmds): robot_pos = robot.move(cmds[i], robot_pos, dt / step) if (i % int(step) == 0): diff --git a/modules/python/examples/ukf-nonlinear-example.py b/modules/python/examples/ukf-nonlinear-example.py index 65cdfff282..73d65a0199 100644 --- a/modules/python/examples/ukf-nonlinear-example.py +++ b/modules/python/examples/ukf-nonlinear-example.py @@ -66,7 +66,6 @@ from visp.core import ColVector, Matrix, UnscentedKalman, UKSigmaDrawerMerwe, Math import numpy as np from typing import List -from math import cos, sin # For the Graphical User Interface try: @@ -76,80 +75,80 @@ has_gui = False import numpy as np -def normalizeAngle(angle: float) -> float: - angleIn0to2pi = Math.modulo(angle, 2. * np.pi) - angleInMinPiPi = angleIn0to2pi - if angleInMinPiPi > np.pi: +def normalize_angle(angle: float) -> float: + angle_0_to_2pi = Math.modulo(angle, 2. * np.pi) + angle_MinPi_Pi = angle_0_to_2pi + if angle_MinPi_Pi > np.pi: # Substract 2 PI to be in interval [-Pi; Pi] - angleInMinPiPi = angleInMinPiPi - 2. * np.pi - return angleInMinPiPi + angle_MinPi_Pi = angle_MinPi_Pi - 2. * np.pi + return angle_MinPi_Pi -def measurementMean(measurements: List[ColVector], wm: List[float]) -> ColVector: +def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVector: """ - @brief Compute the weighted mean of measurement vectors. + Compute the weighted mean of measurement vectors. - @param[in] measurements The measurement vectors, such as measurements[i][0] = range and + :param measurements: The measurement vectors, such as measurements[i][0] = range and measurements[i][1] = elevation_angle. - @param[in] wm The associated weights. + :param wm: The associated weights. - @return vpColVector + :return vpColVector: The weighted mean of the measurement vectors. """ - nbPoints = len(measurements) - sumCos = 0. - sumSin = 0. + nb_points = len(measurements) + sum_cos = 0. + sum_sin = 0. meanRange = 0. - for i in range(nbPoints): + for i in range(nb_points): meanRange += wm[i] * measurements[i][0] - sumCos += wm[i] * cos(measurements[i][1]) - sumSin += wm[i] * sin(measurements[i][1]) + sum_cos += wm[i] * np.cos(measurements[i][1]) + sum_sin += wm[i] * np.sin(measurements[i][1]) - meanAngle = np.arctan2(sumSin, sumCos) + mean_angle = np.arctan2(sum_sin, sum_cos) - return ColVector([meanRange, meanAngle]) + return ColVector([meanRange, mean_angle]) -def measurementResidual(meas: ColVector, toSubstract: ColVector) -> ColVector: +def measurementResidual(meas: ColVector, to_substract: ColVector) -> ColVector: """ - @brief Compute the substraction between two vectors expressed in the measurement space, + Compute the substraction between two vectors expressed in the measurement space, such as v[0] = range ; v[1] = elevation_angle - @param[in] meas Measurement to which we must substract something. - @param[in] toSubstract The something we must substract. + :param meas: Measurement to which we must substract something. + :param toSubstract: The something we must substract. - @return vpColVector \b meas - \b toSubstract . + :return vpColVector: \b meas - \b toSubstract . """ - resTemp = meas - toSubstract - return ColVector([resTemp[0], normalizeAngle(resTemp[1])]) + res_temp = meas - to_substract + return ColVector([res_temp[0], normalize_angle(res_temp[1])]) -def add_state_vectors(a, b) -> ColVector: +def state_add_vectors(a, b) -> ColVector: """ - @brief Method that permits to add two state vectors. + Method that permits to add two state vectors. - @param a The first state vector to which another state vector must be added. - @param b The other state vector that must be added to a. + :param a: The first state vector to which another state vector must be added. + :param b: The other state vector that must be added to a. - @return ColVector The sum a + b. + :return ColVector: The sum a + b. """ return a + b -def residual_state_vectors(a, b) -> ColVector: +def state_residual_vectors(a, b) -> ColVector: """ - @brief Method that permits to substract a state vector to another. + Method that permits to substract a state vector to another. - @param a The first state vector to which another state vector must be substracted. - @param b The other state vector that must be substracted to a. + :param a: The first state vector to which another state vector must be substracted. + :param b: The other state vector that must be substracted to a. - @return ColVector The substraction a - b. + :return ColVector: The substraction a - b. """ return a - b def fx(x: ColVector, dt: float) -> ColVector: """ - @brief Process function that projects in time the internal state of the UKF. + Process function that projects in time the internal state of the UKF. - @param x The internal state of the UKF. - @param dt The sampling time: how far in the future are we projecting x. + :param x: The internal state of the UKF. + :param dt: The sampling time: how far in the future are we projecting x. - @return ColVector The updated internal state, projected in time, also known as the prior. + :return ColVector: The updated internal state, projected in time, also known as the prior. """ return ColVector([ x[0] + dt * x[1], @@ -160,106 +159,106 @@ def fx(x: ColVector, dt: float) -> ColVector: class vpRadarStation: """ - @brief Class that permits to convert the position of the aircraft into + Class that permits to convert the position of the aircraft into range and elevation angle measurements. """ - def __init__(self, x, y, range_std, elev_angle_std): + def __init__(self, x: float, y: float, range_std: float, elev_angle_std: float): """ - @brief Construct a new vpRadarStation + Construct a new vpRadarStation - @param x The position on the ground of the radar. - @param y The altitude of the radar. - @param range_std The standard deviation of the range measurements. - @param elev_angle_std The standard deviation of the elevation angle measurements. + :param x: The position on the ground of the radar. + :param y: The altitude of the radar. + :param range_std: The standard deviation of the range measurements. + :param elev_angle_std: The standard deviation of the elevation angle measurements. """ - self.m_x = x - self.m_y = y - self.m_stdevRange = range_std - self.m_stdevElevAngle = elev_angle_std + self._x = x + self._y = y + self._stdevRange = range_std + self._stdevElevAngle = elev_angle_std - def state_to_measurement(self, x) -> ColVector: + def state_to_measurement(self, x: ColVector) -> ColVector: """ - @brief Measurement function that expresses the internal state of the UKF in the measurement space. + Measurement function that expresses the internal state of the UKF in the measurement space. - @param x The internal state of the UKF. + :param x: The internal state of the UKF. - @return ColVector The internal state, expressed in the measurement space. + :return ColVector: The internal state, expressed in the measurement space. """ - dx = x[0] - self.m_x - dy = x[2] - self.m_y + dx = x[0] - self._x + dy = x[2] - self._y range = np.sqrt(dx * dx + dy * dy) elev_angle = np.arctan2(dy, dx) return ColVector([range, elev_angle]) - def measureGT(self, pos) -> ColVector: + def measure_gt(self, pos: ColVector) -> ColVector: """ - @brief Perfect measurement of the range and elevation angle that + Perfect measurement of the range and elevation angle that correspond to pos. - @param pos The actual position of the aircraft (pos[0]: projection of the position + :param pos: The actual position of the aircraft (pos[0]: projection of the position on the ground, pos[1]: altitude). - @return ColVector [0] the range [1] the elevation angle. + :return ColVector: [0] the range [1] the elevation angle. """ - dx = pos[0] - self.m_x - dy = pos[1] - self.m_y + dx = pos[0] - self._x + dy = pos[1] - self._y range = np.sqrt(dx * dx + dy * dy) elev_angle = np.arctan2(dy, dx) return ColVector([range, elev_angle]) - def measureWithNoise(self, pos: ColVector) -> ColVector: + def measure_with_noise(self, pos: ColVector) -> ColVector: """ - @brief Noisy measurement of the range and elevation angle that + Noisy measurement of the range and elevation angle that correspond to pos. - @param pos The actual position of the aircraft (pos[0]: projection of the position + :param pos: The actual position of the aircraft (pos[0]: projection of the position on the ground, pos[1]: altitude). - @return vpColVector [0] the range [1] the elevation angle. + :return vpColVector: [0] the range [1] the elevation angle. """ - measurementsGT = self.measureGT(pos) - measurementsNoisy = ColVector([measurementsGT[0] + np.random.normal(0., self.m_stdevRange), measurementsGT[1] + np.random.normal(0., self.m_stdevElevAngle)]) - return measurementsNoisy + measurements_GT = self.measure_gt(pos) + measurements_noisy = ColVector([measurements_GT[0] + np.random.normal(0., self._stdevRange), measurements_GT[1] + np.random.normal(0., self._stdevElevAngle)]) + return measurements_noisy class vpACSimulator: """ - @brief Class to simulate a flying aircraft. + Class to simulate a flying aircraft. """ def __init__(self, X0: ColVector, vel: ColVector, vel_std: float): """ - @brief Construct a new vpACSimulator object. + Construct a new vpACSimulator object. - @param X0 Initial position of the aircraft. - @param vel Velocity of the aircraft. - @param vel_std Standard deviation of the variation of the velocity. + :param X0: Initial position of the aircraft. + :param vel: Velocity of the aircraft. + :param vel_std: Standard deviation of the variation of the velocity. """ - self.m_pos = X0 # Position of the simulated aircraft - self.m_vel = vel # Velocity of the simulated aircraft + self._pos = X0 # Position of the simulated aircraft + self._vel = vel # Velocity of the simulated aircraft np.random.seed(4224) - self.m_stdevVel = vel_std # Standard deviation of the random generator for slight variations of the velocity of the aircraft + self._stdevVel = vel_std # Standard deviation of the random generator for slight variations of the velocity of the aircraft def update(self, dt: float) -> ColVector: """ - \brief Compute the new position of the aircraft after dt seconds have passed + Compute the new position of the aircraft after dt seconds have passed since the last update. - \param[in] dt Period since the last update. - \return vpColVector The new position of the aircraft. + :param dt: Period since the last update. + :return ColVector: The new position of the aircraft. """ - dx_temp = self.m_vel * dt - dx = ColVector([dx_temp[0] + np.random.normal(0., self.m_stdevVel) * dt, dx_temp[1] + np.random.normal(0., self.m_stdevVel) * dt]) - self.m_pos += dx - return self.m_pos + dx_temp = self._vel * dt + dx = ColVector([dx_temp[0] + np.random.normal(0., self._stdevVel) * dt, dx_temp[1] + np.random.normal(0., self._stdevVel) * dt]) + self._pos += dx + return self._pos def generate_Q_matrix(dt: float) -> Matrix: """ - @brief Method that generates the process covariance matrix for a process for which the + Method that generates the process covariance matrix for a process for which the state vector can be written as (x, dx/dt)^T - @param dt The sampling period. + :param dt: The sampling period. - @return Matrix The corresponding process covariance matrix. + :return Matrix: The corresponding process covariance matrix. """ return Matrix( [[dt**3/3, dt**2/2, 0, 0], @@ -269,7 +268,7 @@ def generate_Q_matrix(dt: float) -> Matrix: def generate_P0_matrix() -> Matrix: """ - @brief Method that generates the intial guess of the state covariance matrix. + Method that generates the intial guess of the state covariance matrix. @return Matrix The corresponding state covariance matrix. """ @@ -285,26 +284,26 @@ def generate_P0_matrix() -> Matrix: gt_Y_init = 1000. # Ground truth initial position along the Y-axis, in meters gt_vX_init = 100. # The velocity along the x-axis gt_vY_init = 5. # The velocity along the y-axis - procVar = 0.1 # The variance of the process function - sigmaRange = 5 # Standard deviation of the range measurement: 5m - sigmaElevAngle = Math.rad(0.5) # Standard deviation of the elevation angle measurent: 0.5deg - stdevAircraftVelocity = 0.2; # Standard deviation of the velocity of the simulated aircraft, + proc_var = 0.1 # The variance of the process function + sigma_range = 5 # Standard deviation of the range measurement: 5m + sigma_elev_angle = Math.rad(0.5) # Standard deviation of the elevation angle measurent: 0.5deg + stdev_aircraft_velocity = 0.2; # Standard deviation of the velocity of the simulated aircraft, # to make it deviate a bit from the constant velocity model # The object that draws the sigma points used by the UKF - drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=residual_state_vectors, addFunc=add_state_vectors) + drawer = UKSigmaDrawerMerwe(n=4, alpha=0.3, beta=2, kappa=-1, resFunc=state_residual_vectors, addFunc=state_add_vectors) # The object that performs radar measurements - radar = vpRadarStation(0., 0., sigmaRange, sigmaElevAngle) + radar = vpRadarStation(0., 0., sigma_range, sigma_elev_angle) P0 = generate_P0_matrix() # The initial estimate of the state covariance matrix - R = Matrix([[sigmaRange * sigmaRange, 0], [0, sigmaElevAngle * sigmaElevAngle]]) # The measurement covariance matrix - Q = generate_Q_matrix(dt) * procVar # The process covariance matrix + R = Matrix([[sigma_range * sigma_range, 0], [0, sigma_elev_angle * sigma_elev_angle]]) # The measurement covariance matrix + Q = generate_Q_matrix(dt) * proc_var # The process covariance matrix ukf = UnscentedKalman(Q, R, drawer, fx, radar.state_to_measurement) # The Unscented Kalman Filter instance # Initializing the state vector and state covariance matrix estimates ukf.init(ColVector([0.9 * gt_X_init, 0.9 * gt_vX_init, 0.9 * gt_Y_init, 0.9 * gt_vY_init]), P0) - ukf.setMeasurementMeanFunction(measurementMean) + ukf.setMeasurementMeanFunction(measurement_mean) ukf.setMeasurementResidualFunction(measurementResidual) # Initializing the Graphical User Interface if the needed libraries are available @@ -332,13 +331,13 @@ def generate_P0_matrix() -> Matrix: ac_pos = ColVector([gt_X_init, gt_Y_init]) # Ground truth position ac_vel = ColVector([gt_vX_init, gt_vY_init]) # Ground truth velocity - ac = vpACSimulator(ac_pos, ac_vel, stdevAircraftVelocity) - gt_Xprec = ColVector([ac_pos[0], ac_pos[1]]) + ac = vpACSimulator(ac_pos, ac_vel, stdev_aircraft_velocity) + gt_X_prev = ColVector([ac_pos[0], ac_pos[1]]) # Previous ground truth position for i in range(500): # Creating noisy measurements gt_X = ac.update(dt) - gt_V = (gt_X - gt_Xprec) / dt - z = radar.measureWithNoise(gt_X) + gt_V = (gt_X - gt_X_prev) / dt + z = radar.measure_with_noise(gt_X) # Filtering using the UKF ukf.filter(z, dt) @@ -361,7 +360,7 @@ def generate_P0_matrix() -> Matrix: plot.plot(3, 1, i, Xest[3]) # Updating last measurement for future computation of the noisy velocity - gt_Xprec = ColVector([gt_X[0], gt_X[1]]) + gt_X_prev = ColVector([gt_X[0], gt_X[1]]) print('Finished') input('Press enter to quit') From 03458dccd50905826085567c8644323c9dddf8a1 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Thu, 30 May 2024 10:26:10 +0200 Subject: [PATCH 10/16] [PYTHON][EXAMPLE] Vectorized a loop --- .../examples/ukf-nonlinear-complex-example.py | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index de4e03a93e..0fea57bbfa 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -159,15 +159,13 @@ def state_mean_vectors(states: List[ColVector], wm: List[float]) -> ColVector: :return ColVector: The weighted mean. """ mean = np.zeros(3) - nbPoints = len(states) - sumCos = 0. - sumSin = 0. - for i in range (nbPoints): - mean[0] += wm[i] * states[i][0] - mean[1] += wm[i] * states[i][1] - sumCos += wm[i] * np.cos(states[i][2]) - sumSin += wm[i] * np.sin(states[i][2]) - + wm_np = np.array(wm) + weighted_x = wm_np * np.array([state[0] for state in states]) + weighted_y = wm_np * np.array([state[1] for state in states]) + mean[0] = np.array(weighted_x).sum() + mean[1] = np.array(weighted_y).sum() + sumCos = (wm_np * np.array([np.cos(state[2]) for state in states])).sum() + sumSin = (wm_np * np.array([np.sin(state[2]) for state in states])).sum() mean[2] = np.arctan2(sumSin, sumCos) return ColVector(mean) @@ -207,9 +205,9 @@ def generate_turn_commands(v: float, angleStart: float, angleStop: float, nbStep :return List[ColVector]: The corresponding list of commands. """ cmds = [] - dTheta = Math.rad(angleStop - angleStart) / float(nbSteps - 1); + dTheta = Math.rad(angleStop - angleStart) / float(nbSteps - 1) for i in range(nbSteps): - theta = Math.rad(angleStart) + dTheta * float(i); + theta = Math.rad(angleStart) + dTheta * float(i) cmd = ColVector([v, theta]) cmds.append(cmd) return cmds From d6e7ac2fe1d9566b513a94728549ec40f1ffe4d4 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Thu, 30 May 2024 10:30:03 +0200 Subject: [PATCH 11/16] [PYTHON][EXAMPLE] Vectorized a loop --- modules/python/examples/ukf-nonlinear-example.py | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/modules/python/examples/ukf-nonlinear-example.py b/modules/python/examples/ukf-nonlinear-example.py index 73d65a0199..15a4b6ea50 100644 --- a/modules/python/examples/ukf-nonlinear-example.py +++ b/modules/python/examples/ukf-nonlinear-example.py @@ -93,15 +93,12 @@ def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVecto :return vpColVector: The weighted mean of the measurement vectors. """ - nb_points = len(measurements) - sum_cos = 0. - sum_sin = 0. - meanRange = 0. - for i in range(nb_points): - meanRange += wm[i] * measurements[i][0] - sum_cos += wm[i] * np.cos(measurements[i][1]) - sum_sin += wm[i] * np.sin(measurements[i][1]) - + wm_np = np.array(wm) + ranges = np.array([meas[0] for meas in measurements]) + elev_angles = np.array([meas[1] for meas in measurements]) + meanRange = (wm_np * ranges).sum() + sum_cos = (wm_np * np.cos(elev_angles)).sum() + sum_sin = (wm_np * np.sin(elev_angles)).sum() mean_angle = np.arctan2(sum_sin, sum_cos) return ColVector([meanRange, mean_angle]) From d1cd200e067e1027fddaf8960bb4b1a65f5c85da Mon Sep 17 00:00:00 2001 From: rlagneau Date: Mon, 3 Jun 2024 10:08:18 +0200 Subject: [PATCH 12/16] [DOC] Fixed UKF documentation --- doc/tutorial/misc/tutorial-ukf.dox | 72 ++++++++++++------- example/kalman/ukf-linear-example.cpp | 12 ++-- .../kalman/ukf-nonlinear-complex-example.cpp | 12 ++-- example/kalman/ukf-nonlinear-example.cpp | 12 ++-- .../include/visp3/core/vpUKSigmaDrawerMerwe.h | 7 +- .../include/visp3/core/vpUnscentedKalman.h | 25 ++++--- modules/python/examples/ukf-linear-example.py | 12 ++-- .../examples/ukf-nonlinear-complex-example.py | 12 ++-- .../python/examples/ukf-nonlinear-example.py | 12 ++-- tutorial/kalman/tutorial-ukf.cpp | 14 ++-- 10 files changed, 126 insertions(+), 64 deletions(-) diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox index 6ade91180e..2b08c92a61 100644 --- a/doc/tutorial/misc/tutorial-ukf.dox +++ b/doc/tutorial/misc/tutorial-ukf.dox @@ -13,11 +13,13 @@ frame is denoted \f$ {F}_C \f$. The object is supposed plane and having four mar The equations that describe the motion of the object in the world frame are the following: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^W \textbf{X}_x &=& R cos(\omega t + \phi) \\ {}^W \textbf{X}_y &=& R sin(\omega t + \phi) \\ {}^W \textbf{X}_z &=& constant -\f} +\end{array} +\f] where \f$ \omega \f$ and \f$ \phi \f$ are respectively the pulsation and the phase of the motion, while \f$ R \f$ is the radius of the revolution around the origin of the world frame. @@ -38,10 +40,12 @@ The first step of the UKF is the prediction step. During this step, some particu \f$ \chi \f$, are drawn along with associated weights \f$ \textbf{w}^m \f$ for the computation of the mean and \f$ \textbf{w}^c \f$ for the computation of the covariance: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) -\f} +\end{array} +\f] There are different ways of drawing the sigma points and associated weights in the litterature, such as the one proposed by Julier or the one proposed by E. A. Wan and R. van der Merwe in \cite Merwe00. @@ -55,11 +59,13 @@ to project them forward in time, forming the new prior: Finally, we apply the Unscented Transform to compute the mean \f$ \boldsymbol{\mu} \f$ and covariance \f$ \overline{\textbf{P}} \f$ of the prior: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \boldsymbol{\mu}, \overline{\textbf{P}} &=& UT({Y}, \textbf{w}^m, \textbf{w}^c, \textbf{Q}) \\ \boldsymbol{\mu} &=& \sum_{i=0}^{2n} w_i^m {Y}_i \\ \overline{\textbf{P}} &=& \sum_{i=0}^{2n} ( w_i^c ({Y}_i - \boldsymbol{\mu}) ({Y}_i - \boldsymbol{\mu})^T ) + \textbf{Q} - \f} + \end{array} +\f] where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function. @@ -71,11 +77,13 @@ the prior into measurements using the measurement function \f$ h: {R}^n \righta Then, we use once again the Unscented Transform to compute the mean \f$ \boldsymbol{\mu}_z \in {R}^m \f$ and the covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \boldsymbol{\mu}_z, \textbf{P}_z &=& UT({Z}, \textbf{w}^m, \textbf{w}^c, \textbf{R}) \\ \boldsymbol{\mu}_z &=& \sum_{i=0}^{2n} w_i^m {Z}_i \\ \textbf{P}_z &=& \sum_{i=0}^{2n} ( w_i^c ({Z}_i - \boldsymbol{\mu}_z) ({Z}_i - \boldsymbol{\mu}_z)^T ) + \textbf{R} -\f} +\end{array} +\f] where \f$ \textbf{R} \f$ is the measurement covariance matrix. @@ -93,10 +101,12 @@ The Kalman's gain is then defined as: Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new covariance \f$ \textbf{P} \f$: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T -\f} +\end{array} +\f] \section tuto-ukf-tutorial Explanations about the tutorial @@ -122,21 +132,25 @@ Press `Return` to leave the program. For this tutorial, we use the main program tutorial-ukf.cpp . The internal state of the UKF is the 3D position of the object expressed in the world frame, along with the pulsation \f$ \omega \f$ of the motion: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \textbf{x}[0] &=& {}^WX_x \\ \textbf{x}[1] &=& {}^WX_y \\ \textbf{x}[2] &=& {}^WX_z \\ \textbf{x}[3] &=& \omega \Delta t -\f} +\end{array} +\f] The measurement \f$ \textbf{z} \f$ corresponds to the perspective projection of the different markers in the image. Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker. The measurement vector can be written as: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \textbf{z}[2i] &=& u_i \\ \textbf{z}[2i+1] &=& v_i -\f} +\end{array} +\f] Be \f$ \textbf{K}_{intr} \f$ the camera instrinsic parameters matrix defined by: @@ -194,42 +208,52 @@ previous state \f$ \textbf{x}_{t} \f$. As stated in the \ref tuto-ukf-intro, the equations of motion of the object are the following: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^W X_x(t) &=& R cos(\omega t + \phi) \\ {}^W X_y(t) &=& R sin(\omega t + \phi) \\ {}^W X_z(t) &=& constant -\f} +\end{array} +\f] Thus, we have: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^WX_x( t + \Delta t) &=& R cos(\omega (t + \Delta t) + \phi) &=& R cos((\omega t + \phi) + \omega \Delta t )\\ {}^WX_y( t + \Delta t) &=& R sin(\omega (t + \Delta t) + \phi) &=& R sin((\omega t + \phi) + \omega \Delta t )\\ {}^WX_z( t + \Delta t) &=& constant -\f} +\end{array} +\f] Which can be rewritten: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^WX_x( t + \Delta t) &=& R cos((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) \\ {}^WX_y( t + \Delta t) &=& R sin((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t)\\ {}^WX_z( t + \Delta t) &=& constant -\f} +\end{array} +\f] And can be finally written as: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} {}^WX_x( t + \Delta t) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) &=& {}^W X_x( t) cos(\omega \Delta t) - {}^W X_y(t) sin(\omega \Delta t) \\ {}^WX_y( t + \Delta t) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t) &=& {}^W X_x( t) sin(\omega \Delta t) + {}^W X_y(t) cos(\omega \Delta t) \\ {}^WX_z( t + \Delta t) &=& constant -\f} +\end{array} +\f] This motivates us to choose the following non-linear process function: -\f{eqnarray*}{ +\f[ + \begin{array}{lcl} \textbf{x}[0]_{t + \Delta t} &=& {}^WX_x (t + \Delta t) &=& \textbf{x}[0]_{t} cos(\textbf{x}[3]_{t}) - \textbf{x}[1]_{t} sin(\textbf{x}[3]_{t}) \\ \textbf{x}[1]_{t + \Delta t} &=& {}^WX_y (t + \Delta t) &=& \textbf{x}[0]_{t} sin(\textbf{x}[3]_{t}) + \textbf{x}[1]_{t} cos(\textbf{x}[3]_{t}) \\ \textbf{x}[2]_{t + \Delta t} &=& {}^WX_z (t + \Delta t) &=& \textbf{x}[2]_{t} \\ \textbf{x}[3]_{t + \Delta t} &=& \omega \Delta t &=& \textbf{x}[3]_{t} -\f} +\end{array} +\f] As the process function is pretty simple, a simple function called here `fx()` is enough: diff --git a/example/kalman/ukf-linear-example.cpp b/example/kalman/ukf-linear-example.cpp index 7ff71e57b9..0a67b11b6c 100644 --- a/example/kalman/ukf-linear-example.cpp +++ b/example/kalman/ukf-linear-example.cpp @@ -37,19 +37,23 @@ * The system we are interested in is a system moving on a 2D-plane. * * The state vector of the UKF is: - * \f{eqnarray*}{ + * \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& \dot{x} \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \dot{y} - \f} + \end{array} + \f] * The measurement \f$ \textbf{z} \f$ corresponds to the position along the x-axis * and y-axis. The measurement vector can be written as: - * \f{eqnarray*}{ + * \f[ + \begin{array}{lcl} \textbf{z}[0] &=& x \\ \textbf{z}[1] &=& y - \f} + \end{array} + \f] * Some noise is added to the measurement vector to simulate a sensor which is * not perfect. diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp index cbe9be9069..3dfb172c31 100644 --- a/example/kalman/ukf-nonlinear-complex-example.cpp +++ b/example/kalman/ukf-nonlinear-complex-example.cpp @@ -36,20 +36,24 @@ * As such, it can be modeled using a bicycle model. * * The state vector of the UKF is: - * \f{eqnarray*}{ + * \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \theta - \f} + \end{array} + \f] where \f$ \theta \f$ is the heading of the robot. The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark along the x and y axis, the measurement vector can be written as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta - \f} + \end{array} + \f] * Some noise is added to the measurement vector to simulate measurements which are * not perfect. diff --git a/example/kalman/ukf-nonlinear-example.cpp b/example/kalman/ukf-nonlinear-example.cpp index eb4f88e5e1..916359749f 100644 --- a/example/kalman/ukf-nonlinear-example.cpp +++ b/example/kalman/ukf-nonlinear-example.cpp @@ -43,20 +43,24 @@ * and the y-axis to the altitude. * * The state vector of the UKF corresponds to a constant velocity model and can be written as: - * \f{eqnarray*}{ + * \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& \dot{x} \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \dot{y} - \f} + \end{array} + \f] The measurement \f$ \textbf{z} \f$ corresponds to the distance and angle between the ground and the aircraft observed by the radar station. Be \f$ p_x \f$ and \f$ p_y \f$ the position of the radar station along the x and y axis, the measurement vector can be written as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[0] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ \textbf{z}[1] &=& \tan^{-1}{\frac{y - p_y}{x - p_x}} - \f} + \end{array} + \f] * Some noise is added to the measurement vector to simulate a sensor which is * not perfect. diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h index de2e9ba1b8..3e3bbdb958 100644 --- a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h @@ -54,12 +54,14 @@ Be \f$ \boldsymbol{\mu} \in {R}^n \f$ the mean and \f$ \boldsymbol{\Sigma} \in {R}^{n x n} \f$ the covariance matrix of the input of the algorithm. The algorithm will draw \f$ 2n + 1 \f$ sigma points \f$ \chi_i \in {R}^n \f$ such as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \chi_0 &=& \boldsymbol{\mu} \\ \chi_i &=& \begin{cases} \boldsymbol{\mu} + \left[ \sqrt{(n + \lambda) \boldsymbol{\Sigma}} \right]_i^T & i = 1 .. n \\ \boldsymbol{\mu} - \left[ \sqrt{(n + \lambda) \boldsymbol{\Sigma}} \right]_{i - n}^T & i = n + 1 .. 2n \end{cases} - \f} + \end{array} + \f] where the subscript \f$ i \f$ denotes that we keep the \f$ i^{th} \f$ of the matrix. @@ -89,7 +91,6 @@ \b Additionnal \b note: the original author recommended to set \f$ \beta = 2 \f$ for Gaussian problems, \f$ \kappa = 3 - n \f$ and \f$ 0 \leq \alpha \leq 1 \f$, where a larger value for \f$ \alpha \f$ spreads the sigma points further from the mean, which can be a problem for highly non-linear problems. - */ class VISP_EXPORT vpUKSigmaDrawerMerwe : public vpUKSigmaDrawerAbstract { diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index 60c3eba162..e2db602c4d 100644 --- a/modules/core/include/visp3/core/vpUnscentedKalman.h +++ b/modules/core/include/visp3/core/vpUnscentedKalman.h @@ -61,10 +61,12 @@ Be \f$ \textbf{x} \in {R}^n \f$ the internal state of the UKF and \f$ \textbf{P} \in {R}^{n\text{ x }n} \f$ the process covariance matrix. We have: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) - \f} + \end{array} + \f] There are different ways of drawing the sigma points and associated weights in the litterature, such as the one proposed by Julier or the one proposed by E. A. Wan and R. van der Merwe. @@ -79,11 +81,13 @@ Then, we apply the Unscented Transform to compute the mean \f$ \boldsymbol{\mu} \f$ and covariance \f$ \overline{\textbf{P}} \f$ of the prior: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \boldsymbol{\mu}, \overline{\textbf{P}} &=& UT({Y}, \textbf{w}^m, \textbf{w}^c, \textbf{Q}) \\ \boldsymbol{\mu} &=& \sum_{i=0}^{2n} w_i^m {Y}_i \\ \overline{\textbf{P}} &=& \sum_{i=0}^{2n} ( w_i^c ({Y}_i - \boldsymbol{\mu}) ({Y}_i - \boldsymbol{\mu})^T ) + \textbf{Q} - \f} + \end{array} + \f] where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function. @@ -95,11 +99,13 @@ Then, we use once again the Unscented Transform to compute the mean \f$ \boldsymbol{\mu}_z \in {R}^m \f$ and the covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \boldsymbol{\mu}_z, \textbf{P}_z &=& UT({Z}, \textbf{w}^m, \textbf{w}^c, \textbf{R}) \\ \boldsymbol{\mu}_z &=& \sum_{i=0}^{2n} w_i^m {Z}_i \\ \textbf{P}_z &=& \sum_{i=0}^{2n} ( w_i^c ({Z}_i - \boldsymbol{\mu}_z) ({Z}_i - \boldsymbol{\mu}_z)^T ) + \textbf{R} - \f} + \end{array} + \f] where \f$ \textbf{R} \f$ is the measurement covariance matrix. @@ -117,11 +123,12 @@ Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new covariance \f$ \textbf{P} \f$: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T - \f} - + \end{array} + \f] */ class VISP_EXPORT vpUnscentedKalman { diff --git a/modules/python/examples/ukf-linear-example.py b/modules/python/examples/ukf-linear-example.py index d0ba4b2db4..b27b7dd7ef 100644 --- a/modules/python/examples/ukf-linear-example.py +++ b/modules/python/examples/ukf-linear-example.py @@ -40,19 +40,23 @@ The system we are interested in is a system moving on a 2D-plane. The state vector of the UKF is: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& \dot{x} \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \dot{y} - \f} + \end{array} + \f] The measurement \f$ \textbf{z} \f$ corresponds to the position along the x-axis and y-axis. The measurement vector can be written as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[0] &=& x \\ \textbf{z}[1] &=& y - \f} + \end{array} + \f] Some noise is added to the measurement vector to simulate a sensor which is not perfect. diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index 0fea57bbfa..29403ef912 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -39,20 +39,24 @@ As such, it can be modeled unp.sing a bicycle model. The state vector of the UKF is: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \theta - \f} + \end{array} + \f] where \f$ \theta \f$ is the heading of the robot. The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark along the x and y axis, the measurement vector can be written as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta - \f} + \end{array} + \f] Some noise is added to the measurement vector to simulate measurements which are not perfect. diff --git a/modules/python/examples/ukf-nonlinear-example.py b/modules/python/examples/ukf-nonlinear-example.py index 15a4b6ea50..bd7062f3a8 100644 --- a/modules/python/examples/ukf-nonlinear-example.py +++ b/modules/python/examples/ukf-nonlinear-example.py @@ -44,20 +44,24 @@ and the y-axis to the altitude. The state vector of the UKF is: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{x}[0] &=& x \\ \textbf{x}[1] &=& \dot{x} \\ \textbf{x}[1] &=& y \\ \textbf{x}[2] &=& \dot{y} - \f} + \end{array} + \f] The measurement \f$ \textbf{z} \f$ corresponds to the distance and angle between the ground and the aircraft observed by the radar station. Be \f$ p_x \f$ and \f$ p_y \f$ the position of the radar station along the x and y axis, the measurement vector can be written as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[0] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ \textbf{z}[1] &=& \tan^{-1}{\frac{y - p_y}{x - p_x}} - \f} + \end{array} + \f] Some noise is added to the measurement vector to simulate a sensor which is not perfect. diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp index 0b11bd9d8a..760bf4467b 100644 --- a/tutorial/kalman/tutorial-ukf.cpp +++ b/tutorial/kalman/tutorial-ukf.cpp @@ -38,20 +38,24 @@ * fixed to the ceiling. * * The state vector of the UKF is: - * \f{eqnarray*}{ + * \f[ + \begin{array}{lcl} \textbf{x}[0] &=& {}^WX_x \\ \textbf{x}[1] &=& {}^WX_y \\ \textbf{x}[2] &=& {}^WX_z \\ \textbf{x}[3] &=& \omega \Delta t - \f} + \end{array} + \f] The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers. Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker. The measurement vector can be written as: - \f{eqnarray*}{ + \f[ + \begin{array}{lcl} \textbf{z}[2i] &=& u_i \\ \textbf{z}[2i+1] &=& v_i - \f} + \end{array} + \f] * Some noise is added to the measurement vector to simulate measurements which are * not perfect. @@ -420,6 +424,8 @@ int main(/*const int argc, const char *argv[]*/) object_pos[3] = 1.; //! [Init_simu] + std::cin.get(); + //! [Simu_loop] for (unsigned int i = 0; i < 200; ++i) { //! [Update obj pose] From bd02c0cec98d5669d3c05b133f77b5be9970def2 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Mon, 3 Jun 2024 14:35:22 +0200 Subject: [PATCH 13/16] [CORE] It is now possible to call successively several times predict --- .../include/visp3/core/vpUnscentedKalman.h | 1 + .../src/math/kalman/vpUnscentedKalman.cpp | 19 ++++++++++++++++++- tutorial/kalman/tutorial-ukf.cpp | 2 -- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index e2db602c4d..e2a1c24f2d 100644 --- a/modules/core/include/visp3/core/vpUnscentedKalman.h +++ b/modules/core/include/visp3/core/vpUnscentedKalman.h @@ -384,6 +384,7 @@ class VISP_EXPORT vpUnscentedKalman return mean; } private: + bool m_hasUpdateBeenCalled; /*!< Set to true when update is called, reset at the beginning of predict.*/ vpColVector m_Xest; /*!< The estimated (i.e. filtered) state variables.*/ vpMatrix m_Pest; /*!< The estimated (i.e. filtered) covariance matrix.*/ vpMatrix m_Q; /*!< The covariance introduced by performing the prediction step.*/ diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp index c873e3b2b7..c849145f61 100644 --- a/modules/core/src/math/kalman/vpUnscentedKalman.cpp +++ b/modules/core/src/math/kalman/vpUnscentedKalman.cpp @@ -40,7 +40,8 @@ #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) vpUnscentedKalman::vpUnscentedKalman(const vpMatrix &Q, const vpMatrix &R, std::shared_ptr &drawer, const vpProcessFunction &f, const vpMeasurementFunction &h) - : m_Q(Q) + : m_hasUpdateBeenCalled(false) + , m_Q(Q) , m_R(R) , m_f(f) , m_h(h) @@ -68,6 +69,21 @@ void vpUnscentedKalman::filter(const vpColVector &z, const double &dt, const vpC void vpUnscentedKalman::predict(const double &dt, const vpColVector &u) { + vpColVector x; + vpMatrix P; + + if (m_hasUpdateBeenCalled) { + // Update is the last function that has been called, starting from the filtered values. + x = m_Xest; + P = m_Pest; + m_hasUpdateBeenCalled = false; + } + else { + // Predict is the last function that has been called, starting from the predicted values. + x = m_mu; + P = m_P; + } + // Drawing the sigma points m_chi = m_sigmaDrawer->drawSigmaPoints(m_Xest, m_Pest); @@ -124,6 +140,7 @@ void vpUnscentedKalman::update(const vpColVector &z) // Updating the estimate m_Xest = m_stateAddFunction(m_mu, m_K * m_measResFunc(z, m_muz)); m_Pest = m_P - m_K * m_Pz * m_K.transpose(); + m_hasUpdateBeenCalled = true; } vpUnscentedKalman::vpUnscentedTransformResult vpUnscentedKalman::unscentedTransform(const std::vector &sigmaPoints, diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp index 760bf4467b..37a480dabd 100644 --- a/tutorial/kalman/tutorial-ukf.cpp +++ b/tutorial/kalman/tutorial-ukf.cpp @@ -424,8 +424,6 @@ int main(/*const int argc, const char *argv[]*/) object_pos[3] = 1.; //! [Init_simu] - std::cin.get(); - //! [Simu_loop] for (unsigned int i = 0; i < 200; ++i) { //! [Update obj pose] From 364f5555c7ca119b3b280595df3977989e596c97 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Mon, 3 Jun 2024 21:21:40 +0200 Subject: [PATCH 14/16] [FIX] It is now possible to call successively several times predict (did a mistake in previous commit --- modules/core/src/math/kalman/vpUnscentedKalman.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp index c849145f61..c1fe2143ec 100644 --- a/modules/core/src/math/kalman/vpUnscentedKalman.cpp +++ b/modules/core/src/math/kalman/vpUnscentedKalman.cpp @@ -85,7 +85,7 @@ void vpUnscentedKalman::predict(const double &dt, const vpColVector &u) } // Drawing the sigma points - m_chi = m_sigmaDrawer->drawSigmaPoints(m_Xest, m_Pest); + m_chi = m_sigmaDrawer->drawSigmaPoints(x, P); // Computation of the attached weights vpUKSigmaDrawerAbstract::vpSigmaPointsWeights weights = m_sigmaDrawer->computeWeights(); From 76fef4efb46c0b7d3c286008822e27541f8b7b44 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Tue, 4 Jun 2024 15:41:39 +0200 Subject: [PATCH 15/16] [CORE] It is now possible to get/set the covariance matrices. --- .../include/visp3/core/vpUnscentedKalman.h | 42 ++++++++++++++++++- .../src/math/kalman/vpUnscentedKalman.cpp | 6 +-- 2 files changed, 44 insertions(+), 4 deletions(-) diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index e2a1c24f2d..f959583c45 100644 --- a/modules/core/include/visp3/core/vpUnscentedKalman.h +++ b/modules/core/include/visp3/core/vpUnscentedKalman.h @@ -288,6 +288,26 @@ class VISP_EXPORT vpUnscentedKalman m_stateResFunc = stateResFunc; } + /** + * \brief Permit to change the covariance introduced at each prediction step. + * + * \param[in] Q The process covariance matrix. + */ + inline void setProcessCovariance(const vpMatrix &Q) + { + m_Q = Q; + } + + /** + * \brief Permit to change the covariance introduced at each update step. + * + * \param[in] R The measurement covariance matrix. + */ + inline void setMeasurementCovariance(const vpMatrix &R) + { + m_R = R; + } + /** * \brief Perform first the prediction step and then the filtering step. * @@ -318,6 +338,26 @@ class VISP_EXPORT vpUnscentedKalman */ void update(const vpColVector &z); + /** + * \brief Get the estimated (i.e. filtered) covariance of the state. + * + * \return vpMatrix The filtered covariance matrix. + */ + inline vpMatrix getPest() const + { + return m_Pest; + } + + /** + * \brief Get the predicted covariance of the state, i.e. the covariance of the prior. + * + * \return vpMatrix The predicted covariance matrix. + */ + inline vpMatrix getPpred() const + { + return m_Ppred; + } + /** * \brief Get the estimated (i.e. filtered) state. * @@ -393,7 +433,7 @@ class VISP_EXPORT vpUnscentedKalman std::vector m_wc; /*!< The weights for the covariance computation.*/ std::vector m_Y; /*!< The projection forward in time of the sigma points according to the process model, called the prior.*/ vpColVector m_mu; /*!< The mean of the prior.*/ - vpMatrix m_P; /*!< The covariance matrix of the prior.*/ + vpMatrix m_Ppred; /*!< The covariance matrix of the prior.*/ vpMatrix m_R; /*!< The covariance introduced by performing the update step.*/ std::vector m_Z; /*!< The sigma points of the prior expressed in the measurement space, called the measurement sigma points.*/ vpColVector m_muz; /*!< The mean of the measurement sigma points.*/ diff --git a/modules/core/src/math/kalman/vpUnscentedKalman.cpp b/modules/core/src/math/kalman/vpUnscentedKalman.cpp index c1fe2143ec..7e8eadf27f 100644 --- a/modules/core/src/math/kalman/vpUnscentedKalman.cpp +++ b/modules/core/src/math/kalman/vpUnscentedKalman.cpp @@ -81,7 +81,7 @@ void vpUnscentedKalman::predict(const double &dt, const vpColVector &u) else { // Predict is the last function that has been called, starting from the predicted values. x = m_mu; - P = m_P; + P = m_Ppred; } // Drawing the sigma points @@ -111,7 +111,7 @@ void vpUnscentedKalman::predict(const double &dt, const vpColVector &u) // Computation of the mean and covariance of the prior vpUnscentedTransformResult transformResults = unscentedTransform(m_Y, m_wm, m_wc, m_Q, m_stateResFunc, m_stateMeanFunc); m_mu = transformResults.m_mu; - m_P = transformResults.m_P; + m_Ppred = transformResults.m_P; } void vpUnscentedKalman::update(const vpColVector &z) @@ -139,7 +139,7 @@ void vpUnscentedKalman::update(const vpColVector &z) // Updating the estimate m_Xest = m_stateAddFunction(m_mu, m_K * m_measResFunc(z, m_muz)); - m_Pest = m_P - m_K * m_Pz * m_K.transpose(); + m_Pest = m_Ppred - m_K * m_Pz * m_K.transpose(); m_hasUpdateBeenCalled = true; } From 39c395414b747b1823c7a633b6b15d9e8aba0a64 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 11 Jun 2024 14:45:09 +0200 Subject: [PATCH 16/16] Improve doc --- doc/tutorial/misc/tutorial-ukf.dox | 28 +++---- example/kalman/ukf-linear-example.cpp | 37 +++++---- .../kalman/ukf-nonlinear-complex-example.cpp | 76 +++++++++---------- example/kalman/ukf-nonlinear-example.cpp | 6 +- .../visp3/core/vpUKSigmaDrawerAbstract.h | 4 +- .../include/visp3/core/vpUKSigmaDrawerMerwe.h | 4 +- .../include/visp3/core/vpUnscentedKalman.h | 4 +- tutorial/kalman/tutorial-ukf.cpp | 70 +++++------------ 8 files changed, 95 insertions(+), 134 deletions(-) diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox index 2b08c92a61..f8e0bb1b8e 100644 --- a/doc/tutorial/misc/tutorial-ukf.dox +++ b/doc/tutorial/misc/tutorial-ukf.dox @@ -18,7 +18,7 @@ The equations that describe the motion of the object in the world frame are the {}^W \textbf{X}_x &=& R cos(\omega t + \phi) \\ {}^W \textbf{X}_y &=& R sin(\omega t + \phi) \\ {}^W \textbf{X}_z &=& constant -\end{array} + \end{array} \f] where \f$ \omega \f$ and \f$ \phi \f$ are respectively the pulsation and the phase of the motion, while \f$ R \f$ is the @@ -44,7 +44,7 @@ The first step of the UKF is the prediction step. During this step, some particu \begin{array}{lcl} \chi &=& sigma-function(\textbf{x}, \textbf{P}) \\ \textbf{w}^m, \textbf{w}^c &=& weight-function(n, parameters) -\end{array} + \end{array} \f] There are different ways of drawing the sigma points and associated weights in the litterature, such as the one @@ -64,7 +64,7 @@ and covariance \f$ \overline{\textbf{P}} \f$ of the prior: \boldsymbol{\mu}, \overline{\textbf{P}} &=& UT({Y}, \textbf{w}^m, \textbf{w}^c, \textbf{Q}) \\ \boldsymbol{\mu} &=& \sum_{i=0}^{2n} w_i^m {Y}_i \\ \overline{\textbf{P}} &=& \sum_{i=0}^{2n} ( w_i^c ({Y}_i - \boldsymbol{\mu}) ({Y}_i - \boldsymbol{\mu})^T ) + \textbf{Q} - \end{array} + \end{array} \f] where \f$ \textbf{Q} \f$ is the covariance of the error introduced by the process function. @@ -82,7 +82,7 @@ covariance \f$ \textbf{P}_z \in {R}^{m\text{ x }m} \f$ of these points: \boldsymbol{\mu}_z, \textbf{P}_z &=& UT({Z}, \textbf{w}^m, \textbf{w}^c, \textbf{R}) \\ \boldsymbol{\mu}_z &=& \sum_{i=0}^{2n} w_i^m {Z}_i \\ \textbf{P}_z &=& \sum_{i=0}^{2n} ( w_i^c ({Z}_i - \boldsymbol{\mu}_z) ({Z}_i - \boldsymbol{\mu}_z)^T ) + \textbf{R} -\end{array} + \end{array} \f] where \f$ \textbf{R} \f$ is the measurement covariance matrix. @@ -103,9 +103,9 @@ Finally, we can compute the new state estimate \f$ \textbf{x} \f$ and the new co \f[ \begin{array}{lcl} - \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ - \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T -\end{array} + \textbf{x} &=& \boldsymbol{\mu} + \textbf{K} \textbf{y} \\ + \textbf{P} &=& \overline{\textbf{P}} - \textbf{K} \textbf{P}_z \textbf{K}^T + \end{array} \f] \section tuto-ukf-tutorial Explanations about the tutorial @@ -138,7 +138,7 @@ the 3D position of the object expressed in the world frame, along with the pulsa \textbf{x}[1] &=& {}^WX_y \\ \textbf{x}[2] &=& {}^WX_z \\ \textbf{x}[3] &=& \omega \Delta t -\end{array} + \end{array} \f] The measurement \f$ \textbf{z} \f$ corresponds to the perspective projection of the different markers in the image. @@ -149,7 +149,7 @@ The measurement vector can be written as: \begin{array}{lcl} \textbf{z}[2i] &=& u_i \\ \textbf{z}[2i+1] &=& v_i -\end{array} + \end{array} \f] Be \f$ \textbf{K}_{intr} \f$ the camera instrinsic parameters matrix defined by: @@ -213,7 +213,7 @@ As stated in the \ref tuto-ukf-intro, the equations of motion of the object are {}^W X_x(t) &=& R cos(\omega t + \phi) \\ {}^W X_y(t) &=& R sin(\omega t + \phi) \\ {}^W X_z(t) &=& constant -\end{array} + \end{array} \f] Thus, we have: @@ -223,7 +223,7 @@ Thus, we have: {}^WX_x( t + \Delta t) &=& R cos(\omega (t + \Delta t) + \phi) &=& R cos((\omega t + \phi) + \omega \Delta t )\\ {}^WX_y( t + \Delta t) &=& R sin(\omega (t + \Delta t) + \phi) &=& R sin((\omega t + \phi) + \omega \Delta t )\\ {}^WX_z( t + \Delta t) &=& constant -\end{array} + \end{array} \f] Which can be rewritten: @@ -232,7 +232,7 @@ Which can be rewritten: {}^WX_x( t + \Delta t) &=& R cos((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) \\ {}^WX_y( t + \Delta t) &=& R sin((\omega t + \phi) + \omega \Delta t ) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t)\\ {}^WX_z( t + \Delta t) &=& constant -\end{array} + \end{array} \f] And can be finally written as: @@ -241,7 +241,7 @@ And can be finally written as: {}^WX_x( t + \Delta t) &=& R cos(\omega t + \phi) cos (\omega \Delta t ) - R sin(\omega t + \phi) sin(\omega \Delta t) &=& {}^W X_x( t) cos(\omega \Delta t) - {}^W X_y(t) sin(\omega \Delta t) \\ {}^WX_y( t + \Delta t) &=& R cos(\omega t + \phi) sin (\omega \Delta t ) + R sin(\omega t + \phi) cos(\omega \Delta t) &=& {}^W X_x( t) sin(\omega \Delta t) + {}^W X_y(t) cos(\omega \Delta t) \\ {}^WX_z( t + \Delta t) &=& constant -\end{array} + \end{array} \f] This motivates us to choose the following non-linear process function: @@ -252,7 +252,7 @@ This motivates us to choose the following non-linear process function: \textbf{x}[1]_{t + \Delta t} &=& {}^WX_y (t + \Delta t) &=& \textbf{x}[0]_{t} sin(\textbf{x}[3]_{t}) + \textbf{x}[1]_{t} cos(\textbf{x}[3]_{t}) \\ \textbf{x}[2]_{t + \Delta t} &=& {}^WX_z (t + \Delta t) &=& \textbf{x}[2]_{t} \\ \textbf{x}[3]_{t + \Delta t} &=& \omega \Delta t &=& \textbf{x}[3]_{t} -\end{array} + \end{array} \f] As the process function is pretty simple, a simple function called here `fx()` is enough: diff --git a/example/kalman/ukf-linear-example.cpp b/example/kalman/ukf-linear-example.cpp index 70d81cf941..664f877cad 100644 --- a/example/kalman/ukf-linear-example.cpp +++ b/example/kalman/ukf-linear-example.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -27,34 +26,32 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ /** \example ukf-linear-example.cpp * Example of a simple linear use-case of the Unscented Kalman Filter (UKF). Using a UKF - * in this case is not necessary, it is done for learning purpous only. + * in this case is not necessary, it is done for learning purpose only. * * The system we are interested in is a system moving on a 2D-plane. * * The state vector of the UKF is: - * \f[ - \begin{array}{lcl} - \textbf{x}[0] &=& x \\ - \textbf{x}[1] &=& \dot{x} \\ - \textbf{x}[1] &=& y \\ - \textbf{x}[2] &=& \dot{y} - \end{array} - \f] - + * \f[ + * \begin{array}{lcl} + * \textbf{x}[0] &=& x \\ + * \textbf{x}[1] &=& \dot{x} \\ + * \textbf{x}[1] &=& y \\ + * \textbf{x}[2] &=& \dot{y} + * \end{array} + * \f] + * * The measurement \f$ \textbf{z} \f$ corresponds to the position along the x-axis * and y-axis. The measurement vector can be written as: * \f[ - \begin{array}{lcl} - \textbf{z}[0] &=& x \\ - \textbf{z}[1] &=& y - \end{array} - \f] - + * \begin{array}{lcl} + * \textbf{z}[0] &=& x \\ + * \textbf{z}[1] &=& y + * \end{array} + * \f] * Some noise is added to the measurement vector to simulate a sensor which is * not perfect. */ diff --git a/example/kalman/ukf-nonlinear-complex-example.cpp b/example/kalman/ukf-nonlinear-complex-example.cpp index 78db38737c..0b0904c36c 100644 --- a/example/kalman/ukf-nonlinear-complex-example.cpp +++ b/example/kalman/ukf-nonlinear-complex-example.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ /** \example ukf-nonlinear-complex-example.cpp * Example of a complex non-linear use-case of the Unscented Kalman Filter (UKF). @@ -36,43 +34,43 @@ * As such, it can be modeled using a bicycle model. * * The state vector of the UKF is: - * \f[ - \begin{array}{lcl} - \textbf{x}[0] &=& x \\ - \textbf{x}[1] &=& y \\ - \textbf{x}[2] &=& \theta - \end{array} - \f] - where \f$ \theta \f$ is the heading of the robot. - - The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the - robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark - along the x and y axis, the measurement vector can be written as: - \f[ - \begin{array}{lcl} - \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ - \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta - \end{array} - \f] - + * \f[ + * \begin{array}{lcl} + * \textbf{x}[0] &=& x \\ + * \textbf{x}[1] &=& y \\ + * \textbf{x}[2] &=& \theta + * \end{array} + * \f] + * where \f$ \theta \f$ is the heading of the robot. + * + * The measurement \f$ \textbf{z} \f$ corresponds to the distance and relative orientation of the + * robot with different landmarks. Be \f$ p_x^i \f$ and \f$ p_y^i \f$ the position of the \f$ i^{th} \f$ landmark + * along the x and y axis, the measurement vector can be written as: + * \f[ + * \begin{array}{lcl} + * \textbf{z}[2i] &=& \sqrt{(p_x^i - x)^2 + (p_y^i - y)^2} \\ + * \textbf{z}[2i+1] &=& \tan^{-1}{\frac{p_y^i - y}{p_x^i - x}} - \theta + * \end{array} + * \f] + * * Some noise is added to the measurement vector to simulate measurements which are * not perfect. - - The mean of several angles must be computed in the Unscented Transform. The definition we chose to use - is the following: - - \f$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n \sin{\theta_i}}{n}, \frac{\sum_{i=1}^n \cos{\theta_i}}{n}) \f$ - - As the Unscented Transform uses a weighted mean, the actual implementation of the weighted mean - of several angles is the following: - - \f$ mean_{weighted}(\boldsymbol{\theta}) = atan2 (\sum_{i=1}^n w_m^i \sin{\theta_i}, \sum_{i=1}^n w_m^i \cos{\theta_i}) \f$ - - where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. - - Additionnally, the addition and substraction of angles must be carefully done, as the result - must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use - the interval \f$[- \pi ; \pi ]\f$ . + * + * The mean of several angles must be computed in the Unscented Transform. The definition we chose to use + * is the following: + * + * \f$ mean(\boldsymbol{\theta}) = atan2 (\frac{\sum_{i=1}^n \sin{\theta_i}}{n}, \frac{\sum_{i=1}^n \cos{\theta_i}}{n}) \f$ + * + * As the Unscented Transform uses a weighted mean, the actual implementation of the weighted mean + * of several angles is the following: + * + * \f$ mean_{weighted}(\boldsymbol{\theta}) = atan2 (\sum_{i=1}^n w_m^i \sin{\theta_i}, \sum_{i=1}^n w_m^i \cos{\theta_i}) \f$ + * + * where \f$ w_m^i \f$ is the weight associated to the \f$ i^{th} \f$ measurements for the weighted mean. + * + * Additionally, the addition and subtraction of angles must be carefully done, as the result + * must stay in the interval \f$[- \pi ; \pi ]\f$ or \f$[0 ; 2 \pi ]\f$ . We decided to use + * the interval \f$[- \pi ; \pi ]\f$ . */ // UKF includes diff --git a/example/kalman/ukf-nonlinear-example.cpp b/example/kalman/ukf-nonlinear-example.cpp index 7305dec272..35d1307a22 100644 --- a/example/kalman/ukf-nonlinear-example.cpp +++ b/example/kalman/ukf-nonlinear-example.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ /** \example ukf-nonlinear-example.cpp * Example of a simple non-linear use-case of the Unscented Kalman Filter (UKF). diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h b/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h index f43dadbb69..f6313fed8d 100644 --- a/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerAbstract.h @@ -31,8 +31,8 @@ * Display a point cloud using PCL library. */ -#ifndef _vpUKSigmaDrawerAbstract_h_ -#define _vpUKSigmaDrawerAbstract_h_ +#ifndef VP_UK_SIGMA_DRAWER_ABSTRACT_H +#define VP_UK_SIGMA_DRAWER_ABSTRACT_H #include diff --git a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h index e52e0c7141..74e84042c3 100644 --- a/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h +++ b/modules/core/include/visp3/core/vpUKSigmaDrawerMerwe.h @@ -31,8 +31,8 @@ * Display a point cloud using PCL library. */ -#ifndef _vpUKSigmaDrawerMerwe_h_ -#define _vpUKSigmaDrawerMerwe_h_ +#ifndef VP_UK_SIGMA_DRAWER_MERWE_H +#define VP_UK_SIGMA_DRAWER_MERWE_H #include diff --git a/modules/core/include/visp3/core/vpUnscentedKalman.h b/modules/core/include/visp3/core/vpUnscentedKalman.h index e1ade56c20..d44a43cd67 100644 --- a/modules/core/include/visp3/core/vpUnscentedKalman.h +++ b/modules/core/include/visp3/core/vpUnscentedKalman.h @@ -31,8 +31,8 @@ * Display a point cloud using PCL library. */ -#ifndef _vpUnscentedKalman_h_ -#define _vpUnscentedKalman_h_ +#ifndef VP_UNSCENTED_KALMAN_H +#define VP_UNSCENTED_KALMAN_H #include diff --git a/tutorial/kalman/tutorial-ukf.cpp b/tutorial/kalman/tutorial-ukf.cpp index 043ae8d3b7..2f552719a2 100644 --- a/tutorial/kalman/tutorial-ukf.cpp +++ b/tutorial/kalman/tutorial-ukf.cpp @@ -1,35 +1,3 @@ -/**************************************************************************** - * - * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2024 by Inria. All rights reserved. - * - * This software is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * See the file LICENSE.txt at the root directory of this source - * distribution for additional information about the GNU GPL. - * - * For using ViSP with software that can not be combined with the GNU - * GPL, please contact Inria about acquiring a ViSP Professional - * Edition License. - * - * See https://visp.inria.fr for more information. - * - * This software was developed at: - * Inria Rennes - Bretagne Atlantique - * Campus Universitaire de Beaulieu - * 35042 Rennes Cedex - * France - * - * If you have questions regarding the use of this file, please contact - * Inria at visp@inria.fr - * - * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ - /** \example tutorial-ukf.cpp * Tutorial on how to use the Unscented Kalman Filter (UKF) on a complex non-linear use-case. * The system is an object, whose coordinate frame origin is the point O, on which are sticked four markers. @@ -38,25 +6,25 @@ * fixed to the ceiling. * * The state vector of the UKF is: - * \f[ - \begin{array}{lcl} - \textbf{x}[0] &=& {}^WX_x \\ - \textbf{x}[1] &=& {}^WX_y \\ - \textbf{x}[2] &=& {}^WX_z \\ - \textbf{x}[3] &=& \omega \Delta t - \end{array} - \f] - - The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers. - Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker. - The measurement vector can be written as: - \f[ - \begin{array}{lcl} - \textbf{z}[2i] &=& u_i \\ - \textbf{z}[2i+1] &=& v_i - \end{array} - \f] - + * \f[ + * \begin{array}{lcl} + * \textbf{x}[0] &=& {}^WX_x \\ + * \textbf{x}[1] &=& {}^WX_y \\ + * \textbf{x}[2] &=& {}^WX_z \\ + * \textbf{x}[3] &=& \omega \Delta t + * \end{array} + * \f] + * + * The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers. + * Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker. + * The measurement vector can be written as: + * \f[ + * \begin{array}{lcl} + * \textbf{z}[2i] &=& u_i \\ + * \textbf{z}[2i+1] &=& v_i + * \end{array} + * \f] + * * Some noise is added to the measurement vector to simulate measurements which are * not perfect. */