You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
@@ -33,19 +33,19 @@ The first decision to make when configuring your sensors is whether or not your
33
33
Fusing Unmeasured Variables
34
34
***************************
35
35
36
-
Let's start with an example. Let's say you have a wheeled, nonholonomic robot that works in a planar environment. Your robot has some wheel encoders that are used to estimate instantaneous X velocity as well as absolute pose information. This information is reported in an `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message. Additionally, your robot has an IMU that measures rotational velocity, vehicle attitude, and linear acceleration. Its data is reported in a `sensor_msgs/Imu.html<http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message. As we are operating in a planar environment, we set the ``two_d_mode`` parameter to *true*. This will automatically zero out all 3D variables, such as :math:`Z`, :math:`roll`, :math:`pitch`, their respective velocities, and :math:`Z` acceleration. We start with this configuration:
36
+
Let's start with an example. Let's say you have a wheeled, nonholonomic robot that works in a planar environment. Your robot has some wheel encoders that are used to estimate instantaneous X velocity as well as absolute pose information. This information is reported in an `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message. Additionally, your robot has an IMU that measures rotational velocity, vehicle attitude, and linear acceleration. Its data is reported in a `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message. As we are operating in a planar environment, we set the ``two_d_mode`` parameter to *true*. This will automatically zero out all 3D variables, such as :math:`Z`, :math:`roll`, :math:`pitch`, their respective velocities, and :math:`Z` acceleration. We start with this configuration:
2. Next, we note that we are not fusing :math:`\dot{Y}`. At first glance, this is the right choice, as our robot cannot move instantaneously sideways. However, if the `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message reports a :math:`0` value for :math:`\dot{Y}` (and the :math:`\dot{Y}` covariance is NOT inflated to a large value), it's best to feed that value to the filter. As a :math:`0` measurement in this case indicates that the robot cannot ever move in that direction, it serves as a perfectly valid measurement:
You may wonder why did we not fuse :math:`\dot{Z}` velocity for the same reason. The answer is that we did when we set ``two_d_mode`` to *false*. If we hadn't, we could, in fact, fuse the :math:`0` measurement for :math:`\dot{Z}` velocity into the filter.
86
+
You may wonder why did we not fuse :math:`\dot{Z}` velocity for the same reason. The answer is that we did when we set ``two_d_mode`` to *false*. If we hadn't, we could, in fact, fuse the :math:`0` measurement for :math:`\dot{Z}` velocity into the filter.
87
87
88
88
3. Last, we come to the IMU. You may notice that we have set the :math:`\ddot{Y}` to *false*. This is due to the fact that many systems, including the hypothetical one we are discussing here, will not undergo instantaneous :math:`Y` acceleration. However, the IMU will likely report non-zero, noisy values for Y acceleration, which can cause your estimate to drift rapidly.
89
89
@@ -96,21 +96,21 @@ The state estimation nodes in ''robot_localization'' allow users to fuse as many
In this case, users should be **very** careful and ensure that the covariances on each measured orientation variable are set correctly. If each IMU advertises a yaw variance of, for example, :math:`0.1`, yet the delta between the IMUs' yaw measurements is :math:`> 0.1`, then the output of the filter will oscillate back and forth between the values provided by each sensor. Users should make sure that the noise distributions around each measurement overlap.
112
112
113
-
2. Alternatively, users can make use of the ``_differential`` parameter. By setting this to *true* for a given sensor, all pose (position and orientation) data is converted to a velocity by calculating the change in the measurement value between two consecutive time steps. The data is then fused as a velocity. Again, though, users should take care: when measurements are fused absolutely (especially IMUs), if the measurement has a static or non-increasing variance for a given variable, then the variance in the estimate's covariance matrix will be bounded. If that information is converted to a velocity, then at each time step, the estimate will gain some small amount of error, and the variance for the variable in question will grow without bound. For position :math:`(X, Y, Z)` information, this isn't an issue, but for orientation data, it is a problem. For example, it is acceptable for a robot to move around its environment and accumulate :math:`1.5` meters of error in the :math:`X` direction after some time. If that same robot moves around and accumulates :math:`1.5` radians of error in yaw, then when the robot next drives forward, its position error will explode.
113
+
2. Alternatively, users can make use of the ``_differential`` parameter. By setting this to *true* for a given sensor, all pose (position and orientation) data is converted to a velocity by calculating the change in the measurement value between two consecutive time steps. The data is then fused as a velocity. Again, though, users should take care: when measurements are fused absolutely (especially IMUs), if the measurement has a static or non-increasing variance for a given variable, then the variance in the estimate's covariance matrix will be bounded. If that information is converted to a velocity, then at each time step, the estimate will gain some small amount of error, and the variance for the variable in question will grow without bound. For position :math:`(X, Y, Z)` information, this isn't an issue, but for orientation data, it is a problem. For example, it is acceptable for a robot to move around its environment and accumulate :math:`1.5` meters of error in the :math:`X` direction after some time. If that same robot moves around and accumulates :math:`1.5` radians of error in yaw, then when the robot next drives forward, its position error will explode.
114
114
115
115
The general rule of thumb for the ``_differential`` parameter is that if a give robot has only one source of orientation data, then the differential parameter should be set to *false*. If there are :math:`N` sources, users can set the ``_differential`` parameter to *true* for :math:`N-1` of them, or simply ensure that the covariance values are large enough to eliminate oscillations.
Copy file name to clipboardExpand all lines: doc/navsat_transform_node.rst
+6-6Lines changed: 6 additions & 6 deletions
Display the source diff
Display the rich diff
Original file line number
Diff line number
Diff line change
@@ -1,7 +1,7 @@
1
1
navsat_transform_node
2
2
*********************
3
3
4
-
``navsat_transform_node`` takes as input a `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message (usually the output of ``ekf_localization_node`` or ``ukf_localization_node``), a `sensor_msgs/Imu.html<http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ containing an accurate estimate of your robot's heading, and a `sensor_msgs/NavSatFix.html<http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing GPS data. It produces an odometry message in coordinates that are consistent with your robot's world frame. This value can be directly fused into your state estimate.
4
+
``navsat_transform_node`` takes as input a `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message (usually the output of ``ekf_localization_node`` or ``ukf_localization_node``), a `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ containing an accurate estimate of your robot's heading, and a `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing GPS data. It produces an odometry message in coordinates that are consistent with your robot's world frame. This value can be directly fused into your state estimate.
5
5
6
6
.. note:: If you fuse the output of this node with any of the state estimation nodes in ``robot_localization``, you should make sure that the ``odomN_differential`` setting is *false* for that input.
7
7
@@ -10,7 +10,7 @@ Parameters
10
10
11
11
~frequency
12
12
^^^^^^^^^^
13
-
The real-valued frequency, in Hz, at which ``navsat_transform_node`` checks for new `sensor_msgs/NavSatFix.html<http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ messages, and publishes filtered `sensor_msgs/NavSatFix.html<http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ when ``publish_filtered_gps`` is set to *true*.
13
+
The real-valued frequency, in Hz, at which ``navsat_transform_node`` checks for new `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ messages, and publishes filtered `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ when ``publish_filtered_gps`` is set to *true*.
14
14
15
15
~delay
16
16
^^^^^^
@@ -30,7 +30,7 @@ If this is *true*, the `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html
30
30
31
31
~publish_filtered_gps
32
32
^^^^^^^^^^^^^^^^^^^^^
33
-
If *true*, ``navsat_transform_node`` will also transform your robot's world frame (e.g., *map*) position back to GPS coordinates, and publish a `sensor_msgs/NavSatFix.html<http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message on the ``/gps/filtered`` topic.
33
+
If *true*, ``navsat_transform_node`` will also transform your robot's world frame (e.g., *map*) position back to GPS coordinates, and publish a `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message on the ``/gps/filtered`` topic.
34
34
35
35
~broadcast_utm_transform
36
36
^^^^^^^^^^^^^^^^^^^^^^^^
@@ -49,17 +49,17 @@ If *true*, ``navsat_transform_node`` will wait to get a datum from either:
49
49
50
50
Subscribed Topics
51
51
=================
52
-
* ``imu/data`` A `sensor_msgs/Imu.html<http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message with orientation data
52
+
* ``imu/data`` A `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message with orientation data
53
53
54
54
* ``odometry/filtered`` A `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message of your robot's current position. This is needed in the event that your first GPS reading comes after your robot has attained some non-zero pose.
55
55
56
-
* ``gps/fix`` A `sensor_msgs/NavSatFix.html<http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing your robot's GPS coordinates
56
+
* ``gps/fix`` A `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing your robot's GPS coordinates
57
57
58
58
Published Topics
59
59
================
60
60
* ``odometry/gps`` A `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message containing the GPS coordinates of your robot, transformed into its world coordinate frame. This message can be directly fused into ``robot_localization``'s state estimation nodes.
61
61
62
-
* ``gps/filtered`` (optional) A `sensor_msgs/NavSatFix.html<http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing your robot's world frame position, transformed into GPS coordinates
62
+
* ``gps/filtered`` (optional) A `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message containing your robot's world frame position, transformed into GPS coordinates
0 commit comments