Academia.eduAcademia.edu

Figure 12 – uploaded by Christoph Hertzberg

See full PDFdownloadDownload figure
Figure 8: Performance of an UKF using a scaled axis rep- resentation for the orientation (top to bottom): Trajectory estimated, error norms as in Fig.|5} NEES, from a single run. The filter operates normally until t = 80s where the orientation approaches the singularity, the filter becomes inconsistent and the error rapidly increases. Surprisingly, the filter recovers in the end, showing that scaled axis is a more robust representation than Euler angles.

Figure 8 Performance of an UKF using a scaled axis rep- resentation for the orientation (top to bottom): Trajectory estimated, error norms as in Fig.|5} NEES, from a single run. The filter operates normally until t = 80s where the orientation approaches the singularity, the filter becomes inconsistent and the error rapidly increases. Surprisingly, the filter recovers in the end, showing that scaled axis is a more robust representation than Euler angles.

Related Figures (18)

Figure 1: Mapping a local neighborhood in the state space (here: on the unit sphere S$?) into R” (here: the plane) al- lows for the use of standard sensor fusion algorithms with- out explicitly encoding the global topological structure.
Figure 1: Mapping a local neighborhood in the state space (here: on the unit sphere S$?) into R” (here: the plane) al- lows for the use of standard sensor fusion algorithms with- out explicitly encoding the global topological structure.
Figure 2: From reality to state representation in the INS example (see text for details). Step 1 models relevant as- pects of reality in the state model. Step 2a uses the stan- dard approach to obtain a lossy state representation. Step 2b uses the -method to obtain a state representation pre- serving the topological structure of the state space (using a faithful representation SO(3)* of SO(3)).
Figure 2: From reality to state representation in the INS example (see text for details). Step 1 models relevant as- pects of reality in the state model. Step 2a uses the stan- dard approach to obtain a lossy state representation. Step 2b uses the -method to obtain a state representation pre- serving the topological structure of the state space (using a faithful representation SO(3)* of SO(3)).
Figure 3: Axiom (11d): The d-distance between «id, and x HH 6g (dashed line) is less or equal to the distance in the parameterized around « (dotted line). used to compute an initial guess for rotations. cause in vector spaces the weighted mean
Figure 3: Axiom (11d): The d-distance between «id, and x HH 6g (dashed line) is less or equal to the distance in the parameterized around « (dotted line). used to compute an initial guess for rotations. cause in vector spaces the weighted mean
The factor 2 is introduced so that the induced metric d(p,q) is the angle between two orienta- tions and the quaternion and matrix fJ-manifolds are isomorphic. It originates from the fact that the quaternion is multiplied to the left and right of a vector when applying a rotation. The won alence of --q causes the atan(||v|| /w) term in instead of atan2(||v|| , w), making log q = log(— mi 4. Least Squares Optimization and Kalman Filtering on H-Manifolds
The factor 2 is introduced so that the induced metric d(p,q) is the angle between two orienta- tions and the quaternion and matrix fJ-manifolds are isomorphic. It originates from the fact that the quaternion is multiplied to the left and right of a vector when applying a rotation. The won alence of --q causes the atan(||v|| /w) term in instead of atan2(||v|| , w), making log q = log(— mi 4. Least Squares Optimization and Kalman Filtering on H-Manifolds
normalized but that is not necessary. The differ- ence in § however, being a plain real value, must
normalized but that is not necessary. The differ- ence in § however, being a plain real value, must
Input, Process and Measurement Models: Table 2: Classical UKF vs. UKF on -manifolds algorithms. See text for details.
Input, Process and Measurement Models: Table 2: Classical UKF vs. UKF on -manifolds algorithms. See text for details.
Table 3: MEANOFSIGMAPOINTS computes the mean of a set of -manifold sigma points Y. In practice, the limit in can be implemented by an iterative loop that is terminated if the norm of the most recent summed error vector is below a certain threshold. The number of sigma points is not necessarily the same as the dimension of each.
Table 3: MEANOFSIGMAPOINTS computes the mean of a set of -manifold sigma points Y. In practice, the limit in can be implemented by an iterative loop that is terminated if the norm of the most recent summed error vector is below a certain threshold. The number of sigma points is not necessarily the same as the dimension of each.
Figure 4: Trajectory estimated from a synthetic dataset by the UKFoM-based minimal INS-GPS filter. The trajectory starts and ends at (0,0) and consists of a stylized figure eight with an embedded stylized loop (right). While on the long segments of the figure eight the helicopter rotates about its roll axis. We also need to implement a function that re-
Figure 4: Trajectory estimated from a synthetic dataset by the UKFoM-based minimal INS-GPS filter. The trajectory starts and ends at (0,0) and consists of a stylized figure eight with an embedded stylized loop (right). While on the long segments of the figure eight the helicopter rotates about its roll axis. We also need to implement a function that re-
Figure 6: Plots for filter consistency evaluation (top to bottom): NEES ||. 8 Xill5,, from a single run; averaged NEES from 50 Monte Carlo runs; Mareen averaged over 50 Monte Carlo runs (NMEE) with & being x, y, and z of position, orientation and velocity . Note how each largely remains within its 95% probability region (dashed), as should be the case for a consistent filter.
Figure 6: Plots for filter consistency evaluation (top to bottom): NEES ||. 8 Xill5,, from a single run; averaged NEES from 50 Monte Carlo runs; Mareen averaged over 50 Monte Carlo runs (NMEE) with & being x, y, and z of position, orientation and velocity . Note how each largely remains within its 95% probability region (dashed), as should be the case for a consistent filter.
Figure 5: Top: Trajectory estimated by the minimal INS- GPS filter (black) vs. ground truth (solid gray) and posi- tion measurements (gray crosses) from a single filter run. The plot shows the z-y-plane (m). Bottom: RMS error norms for position (red, m), orientation (green, rad) and velocity (blue, m/s) estimates from 50 Monte Carlo runs. The time averaged error is 0.415m, 1.58 x 10~-? rad and 0.141 m/s, respectively. This magnitude seems plausible for a op = 0.75m GPS. Also note that there is no particular order in
Figure 5: Top: Trajectory estimated by the minimal INS- GPS filter (black) vs. ground truth (solid gray) and posi- tion measurements (gray crosses) from a single filter run. The plot shows the z-y-plane (m). Bottom: RMS error norms for position (red, m), orientation (green, rad) and velocity (blue, m/s) estimates from 50 Monte Carlo runs. The time averaged error is 0.415m, 1.58 x 10~-? rad and 0.141 m/s, respectively. This magnitude seems plausible for a op = 0.75m GPS. Also note that there is no particular order in
Figure 7: Performance of an UKF using Euler angles for the orientation (top to bottom): Trajectory estimated, er- ror norms as in Fig. [5] NEES, from a single run. The filter operates normally until t = 60s where the orientation ap- proaches the singularity, the filter becomes inconsistent and the error rapidly increases with the estimate leaving the workspace.
Figure 7: Performance of an UKF using Euler angles for the orientation (top to bottom): Trajectory estimated, er- ror norms as in Fig. [5] NEES, from a single run. The filter operates normally until t = 60s where the orientation ap- proaches the singularity, the filter becomes inconsistent and the error rapidly increases with the estimate leaving the workspace.
Figure 9: Performance of an UKF using a plain quaternion (top to bottom): Trajectory estimated, RMS error norms as in Fig. [5] NEES, from 50 Monte Carlo runs. The NEES is slightly too low (ca. by 1), probably because by using o2, as process noise covariance in all 4 quaternion compo- nents, the filter “thinks” there is process noise on the norm of the quaternion, while in fact there is none. The time- averaged error is 0.434_m, 1.71 x 10~? rad, and 0.160 m/s in position, orientation, and velocity, respectively. This is slightly worse than for the H-method, probably caused by the fact, that the quaternion is not fully normalized mak- ing the filter use the false DOF created by the quaternion’s norm to fit to the measurements.
Figure 9: Performance of an UKF using a plain quaternion (top to bottom): Trajectory estimated, RMS error norms as in Fig. [5] NEES, from 50 Monte Carlo runs. The NEES is slightly too low (ca. by 1), probably because by using o2, as process noise covariance in all 4 quaternion compo- nents, the filter “thinks” there is process noise on the norm of the quaternion, while in fact there is none. The time- averaged error is 0.434_m, 1.71 x 10~? rad, and 0.160 m/s in position, orientation, and velocity, respectively. This is slightly worse than for the H-method, probably caused by the fact, that the quaternion is not fully normalized mak- ing the filter use the false DOF created by the quaternion’s norm to fit to the measurements.
Figure 10: Performance of the H-method UKF with col- ored noise (top to bottom): Trajectory estimated, error norms as in Fig. [5] NEES from 50 Monte Carlo runs. The filter is consistent, notably the position error grows over time. This is as expected: The filter knows its initial posi- tion and from this it can initially deduce the GPS-bias with about 1m precision. However, over time the bias drifts and with the inertial system being to imprecise, their is no in- formation on the new bias and hence the position error grows. Velocity and orientation error keep low, because these are deduced from the relative position of GPS mea- surements where the bias cancels out.
Figure 10: Performance of the H-method UKF with col- ored noise (top to bottom): Trajectory estimated, error norms as in Fig. [5] NEES from 50 Monte Carlo runs. The filter is consistent, notably the position error grows over time. This is as expected: The filter knows its initial posi- tion and from this it can initially deduce the GPS-bias with about 1m precision. However, over time the bias drifts and with the inertial system being to imprecise, their is no in- formation on the new bias and hence the position error grows. Velocity and orientation error keep low, because these are deduced from the relative position of GPS mea- surements where the bias cancels out.
Figure 12: Residual sum of squares over iteration steps of Gauss-Newton (GN) and Levenberg-Marquardt (LM) op- timization on the Stanford parking garage data set as used in [14]. Gauss-Newton with Euler-angles is clearly inferior to all other representations, however being far away from singularities, it still converges.
Figure 12: Residual sum of squares over iteration steps of Gauss-Newton (GN) and Levenberg-Marquardt (LM) op- timization on the Stanford parking garage data set as used in [14]. Gauss-Newton with Euler-angles is clearly inferior to all other representations, however being far away from singularities, it still converges.
Figure 11: The DLR data set before (left) and af- ter (right) Gauss-Newton optimization. We also show the residual sum of squares over iteration steps (top, right). No comparison is made, as in 2D the H-operator only en- capsulates the handling of angular periodicity.
Figure 11: The DLR data set before (left) and af- ter (right) Gauss-Newton optimization. We also show the residual sum of squares over iteration steps (top, right). No comparison is made, as in 2D the H-operator only en- capsulates the handling of angular periodicity.
Figure 14: Residual sum of squares over iteration steps of Gauss-Newton and Levenberg-Marquardt with differ- ent state representations on the dataset in Fig. (see Figure [12] for legend). The same data set was optimized using original noise (top) and with additional noises of 0.01 (middle) and 0.1 rad/m (bottom). For the latter two, the median of 31 runs is plotted. Plots ending unfinished indicate that more than half the optimizations could not be finished due to a singularity. The H-method clearly out-performs the singular representations. For the high noise-level the quaternion representation is slightly bet- ter. However, when comparing run-times instead of it- eration counts, the latter is slower due to the additional measurements and the larger state-space (7 DOF instead of 6 DOF per pose). Computation times were 63 ms for the 4D quaternion and 42 ms for the H-approach per step. This fits well to the nominal factor of (7)? + 1.59 for the O(n3)-matrix decomposition. For Euler angle and matrix exponential, the evaluation took longer with times of 80 ms and 62 ms per step probably due to not hand-tuned code and the high number of trigonometric functions involved.
Figure 14: Residual sum of squares over iteration steps of Gauss-Newton and Levenberg-Marquardt with differ- ent state representations on the dataset in Fig. (see Figure [12] for legend). The same data set was optimized using original noise (top) and with additional noises of 0.01 (middle) and 0.1 rad/m (bottom). For the latter two, the median of 31 runs is plotted. Plots ending unfinished indicate that more than half the optimizations could not be finished due to a singularity. The H-method clearly out-performs the singular representations. For the high noise-level the quaternion representation is slightly bet- ter. However, when comparing run-times instead of it- eration counts, the latter is slower due to the additional measurements and the larger state-space (7 DOF instead of 6 DOF per pose). Computation times were 63 ms for the 4D quaternion and 42 ms for the H-approach per step. This fits well to the nominal factor of (7)? + 1.59 for the O(n3)-matrix decomposition. For Euler angle and matrix exponential, the evaluation took longer with times of 80 ms and 62 ms per step probably due to not hand-tuned code and the high number of trigonometric functions involved.
Figure 13: The sphere400 dataset [19] generated by a vir- tual robot driving on a 3D sphere. It consists of a set of 400 three-dimensional poses and about 780 noisy con- straints between them. The constraints stem from motion between consecutive poses or relations to previously vis- ited poses. Poses are initialized from motion constraints (left) and then optimized with our SLoM framework.
Figure 13: The sphere400 dataset [19] generated by a vir- tual robot driving on a 3D sphere. It consists of a set of 400 three-dimensional poses and about 780 noisy con- straints between them. The constraints stem from motion between consecutive poses or relations to previously vis- ited poses. Poses are initialized from motion constraints (left) and then optimized with our SLoM framework.
Figure 15: Spherical distance ||(# 01) G (a H 62)|| (dashed) and Euclidean distance in the tangential plane ||O1 — d2|| (solid) plotted over the angle y between 6, and 62 for the case ||di|| = a = F and |[d2|| = 6 = 5. Confer both sides of for the concrete terms plotted.
Figure 15: Spherical distance ||(# 01) G (a H 62)|| (dashed) and Euclidean distance in the tangential plane ||O1 — d2|| (solid) plotted over the angle y between 6, and 62 for the case ||di|| = a = F and |[d2|| = 6 = 5. Confer both sides of for the concrete terms plotted.
Related topics:
Software EngineeringInformation FusionSensor FusionUnscented Kalman Filter
580 California St., Suite 400
San Francisco, CA, 94104
© 2025 Academia. All rights reserved