REVIEW 3 major objections 4 minor 30 references
Multi-Sensor Fusion for Quadruped Robot State Estimation using Invariant Filtering and Smoothing
T0 review · 3 major / 4 minor · reviewed 2026-08-16 · deepseek-v4-flash
Pith's one-line read The paper claims that LiDAR odometry and GPS position readings can be written as right-invariant observations, letting a quadruped's invariant filter and smoother fuse them and cut position drift.
desk verdict The experimental results are plausible, but the LiDAR/GPS observation models in equations (19)-(20) swap the measurement and constant vector; an absolute position is left-invariant, not right-invariant, so the claimed group-affine guarantees do not hold. read the letter →
The pith
A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.
The reading
What carries the argument
The machinery that carries the argument is the group-affine observation condition of equation (7), $y_t = X_t^{-1}s + w_t^{\text{obs}}$ with $s$ constant, together with the proposed encodings of LiDAR and GPS positions as right-invariant observations: $Y_{\text{lid}} = X^{-1}b_{\text{lid}} + V_{\text{lid}}$ and $Y_{\text{gps}} = X^{-1}b_{\text{gps}} + V_{\text{gps}}$, where $b_{\text{lid}} = [p_{\text{lid}}, 0, 1, 0]^\top$ and $b_{\text{gps}} = [p_{\text{gps}}, 0, 1, 0]^\top$. These write absolute position output as if it were a right-invariant measurement, which is the format the invariant filter/smoother needs to keep the innovation a function of the invariant error alone. A supporting mechanical element is the parallel LiDAR thread that runs a point-to-point ICP algorithm to produce $p_{\text{lid}}$ at roughly 10 Hz while the estimator thread continues at high rate, which is what keeps the added cost to about 0.06 ms per iteration for the filter.
What would settle it
Feed a recorded run back through the estimator with a fixed, time-invariant GPS position while the robot moves, and check whether the innovation and covariance evolve according to the state-independent linear equation (6); if they depend on the robot's pose, the observation is not group-affine and the central claim collapses.
Extended reading notes
Core claim
On its own terms, the paper's discovery is that global position measurements—the output of a LiDAR odometry system and a GPS receiver—can be expressed as right-invariant observations of the form $Y = X^{-1}b + V$, with $b$ a vector that packs the measured position together with zeros and ones, as in equations (19) and (20). Because the observation inherits the right-invariant form of equation (7), the authors argue, the log-linear error dynamics of the invariant filter and smoother are preserved, so the fused estimators keep the convergence guarantees of the invariant filtering theory. The contact-aided kinematic observation is aggregated with these new LiDAR and GPS observations into a single update for the InEKF and a single residual for the IS, and the LiDAR odometry is produced by a point-to-point iterative closest point algorithm running in a separate thread so the main estimator loop stays fast. Experiments on the HOUND2 quadruped, indoors without GPS and outdoors with GPS, show reduced absolute trajectory error and relative position error compared to established LiDAR-inertial odometry systems and to the proprioceptive-only baselines, with the largest visible gain in the z-axis position.
Load-bearing premise
The load-bearing premise is that a LiDAR or GPS position fix can be modeled as a right-invariant observation with a constant vector $b$; if the measured position entering $b$ makes the vector time-varying, the group-affine convergence guarantee does not apply.
Editorial extensions
If this is right
- The fused estimators cut position drift, especially along the z-axis, reporting up to 28% lower error indoors and 40% lower outdoors relative to two established LiDAR-inertial odometry systems.
- The right-invariant encoding of absolute position keeps the estimator's error dynamics state-independent, preserving the convergence behaviour of the invariant filter and smoother.
- The loosely coupled parallel LiDAR thread keeps the filter's computation at about 0.06 ms per iteration, while the smoother's cost grows with its window size, making the accuracy-versus-speed trade-off explicit.
- Ablations show each exteroceptive source contributes: removing GPS increases the outdoor error, and removing LiDAR reverts to the larger drift of the proprioceptive-only configuration.
Reading between the lines
- The parallel-thread design suggests a generic recipe for invariant estimators: any odometry source that emits a position—camera, radar, or learned—could be fused without touching the group-affine core.
- A direct testable extension is to compare the right-invariant encoding against a left-invariant or plain measurement model for absolute position; this would show whether the invariance structure itself drives the reported accuracy gains.
- Since the GPS covariance is treated as fixed in the paper, modulating it with satellite geometry or dilution of precision is a natural next step that could further cut the outdoor error.
- Because the smoother's window size controls both accuracy and computation time, an adaptive window that grows when the robot speed is low or when drift is detected could give the best of both filter and smoother.
Signed reviews
Editorial analysis
A structured set of objections, weighed in public.
Referee Report
Summary. The paper proposes two state-estimation frameworks for quadruped robots, E-InEKF and E-IS, which fuse leg kinematics and IMU with LiDAR odometry (from KISS-ICP) and GPS position measurements. The central claim is that the LiDAR and GPS measurement models, given in Eqs. (19) and (20), satisfy the group-affine property of Eq. (7), thereby preserving the invariant-filtering and invariant-smoothing guarantees from Barrau and Bonnabel and from Yoon et al. The methods are evaluated on the KAIST HOUND2 robot in indoor and outdoor experiments against LIO-SAM, FAST-LIO2, P-InEKF, and P-IS, reporting lower ATE and RPE and reduced z-axis drift.
Significance. If the group-affine observation models were correctly derived, the paper would make a useful contribution: it would extend invariant filtering/smoothing for legged robots to include exteroceptive sensors while retaining the computationally attractive proprioceptive pipeline. The experimental study is also valuable, with comparisons against strong LiDAR-inertial baselines and an ablation of the LiDAR/GPS contributions. However, the theoretical centerpiece of the paper, the claim that Eqs. (19) and (20) are group-affine observations of the form of Eq. (7), is not correct as written; this must be fixed for the contribution to stand.
major comments (3)
- [Sec. III-B, Eqs. (19)-(20)] The LiDAR and GPS observation models are not in the group-affine form of Eq. (7). In Eq. (7), the observation y_t is the measurement (possibly a nonlinear function of the raw measurement) and s is a constant vector. In Eqs. (19) and (20), Y_lid and Y_gps are set to constant vectors, while b_lid and b_gps contain the time-varying measured positions p_lid and p_gps. This is exactly reversed: the measurement enters through s, the supposedly constant vector, and the observation variable is constant. Consequently, the models as written do not satisfy the right-invariant condition y_t = X_t^{-1} s + w_t with constant s, and the log-linear error and convergence guarantees of [7] do not transfer. An absolute position measurement is a left-invariant output of the state, y = X s (with s constant), not a right-invariant one; the paper provides no derivation showing how such a measurement can be cast in the right-invariant form while keeping s constant.
- [Sec. III-D] The E-IS residual and Jacobian definitions inherit the same problem. The residual is r_lid = X Y_lid - b_lid, with b_lid containing the measured position p_lid, and the Jacobian is J_lid = [b_lid^⊙, 0]. Because b_lid is measurement-dependent, the Jacobian depends on the noisy measurement, which is inconsistent with the invariant-smoothing derivation in [12] where the Jacobian is evaluated at a constant vector. The measurement dependence enters the linearized cost and breaks the group-affine structure that the smoother is supposed to exploit. In addition, the text in Sec. III-D says "plid ... represents the LiDAR position in the robot's body frame," which contradicts Sec. III-A where plid is the LiDAR position in the world frame; the paper must clarify which definition is used and reconcile it with the proposed residual.
- [Sec. III-C, Eq. (21)] The aggregated observation model of Eq. (21) does not rescue the formulation. Writing the observation as X_aug^{-1} b with b = [b_kin; b_lid; b_gps] is only valid if each b component is a constant; here b_lid and b_gps contain the measurements, so the right-hand side is not a linear function of a fixed vector. The update equations of Sec. II-C, which were derived for observations of the form of Eq. (7), therefore cannot be applied to this model. The Jacobians H_lid = H_gps = [0,0,-I_3,0] suggest a direct position measurement in world coordinates, which is a left-invariant observation, not the right-invariant form the paper claims. The paper needs to state which invariant structure it actually uses and re-derive the filter and smoother updates accordingly.
minor comments (4)
- [Table II] The column headers for the outdoor results table are run together and missing spaces, making the table hard to read; please reformat.
- [Sec. II-B] The notation "SEN+2(3)" should be written as SE_{N+2}(3) to avoid confusion with SEN as a separate symbol.
- [Sec. III-B] The dimensions of Y_lid and b_lid are not explicitly stated, and it is unclear how the 5-vector [0_{3,1}; 0; 1; 0] is embedded in the higher-dimensional state X_t. This should be spelled out.
- [Sec. I] The statement that this is "the first work to incorporate LiDAR odometry and GPS into InEKF and IS" is broader than the evidence presented; please qualify the novelty claim against the existing invariant-filtering literature.
Circularity Check
LiDAR/GPS 'group-affine' observation models place the measured position inside the supposedly constant vector b, so the right-invariant property is asserted, not derived.
-
self definitional
[Section III-B, Eqs. (19)-(20); propagated to Sec. III-D residuals]
"we express the LiDAR position estimate plid with respect to the state Xt in the right-invariant form as given in (7): Ylid = X−1 t blid + Vlid (19) where blid = [plid 0 1 0]⊤ is the LiDAR observation vector, Ylid = [03,1 0 1 0]⊤ is a constant vector... The introduced observation models for LiDAR and GPS of Equations (19) and (20) fulfill the group-affine property required for the invariant state estimation, because our measurement models follow the right-invariant observation form [7], given in Equation (7)."
Equation (7) defines group-affine observations by y = X^{-1} s + w with s a constant vector. In Eqs. (19)-(20), the paper puts the measured positions p_lid and p_gps inside b_lid and b_gps, so those vectors vary at every time step and the models are not of the form (7). The paper then concludes the models 'fulfill the group-affine property' solely because they are written in the algebraic shape y = X^{-1} b. That is a definitional assertion: the measurement is smuggled into the 'constant' vector, while the declared observation Y is constant and carries no data. The same non-constant b propagates to Sec. III-D, where the residuals r = X Y - b and Jacobians J = b^⊙ become measurement-dependent, contrary to the invariant-smoothing derivation.
full rationale
The paper's real-robot comparisons against LIO-SAM, FAST-LIO2, and the proprioceptive-only invariant estimators are independent empirical evidence, so those experimental results are not circular. However, the stated first-principles contribution, namely deriving LiDAR and GPS observation models that 'satisfy group-affine properties,' reduces to a definitional maneuver: Eqs. (19)-(20) are written in the right-invariant shape of Eq. (7), but the required constant vector s is replaced by measurement-dependent b_lid and b_gps. The claim that the models fulfill Eq. (7) is therefore asserted by construction rather than demonstrated, and the same choice undermines the smoother residuals in Sec. III-D. Because this is a central theoretical claim, but the experimental drift-reduction results stand independently, the overall circularity score is 6.
Assumptions & free parameters
free parameters (4)
- LiDAR position noise covariance Sigma(w_lid)
- GPS position noise covariance Sigma(w_gps)
- E-IS window size WS =
1, 5, 10, 15 in experiments
- Slip rejection covariance adjustment parameters
assumptions (4)
- domain assumption The state propagation model of [8] is group-affine with log-linear error dynamics (equations 9-13).
- standard math The right-invariant observation form y = X^{-1} b + w (equation 7) is sufficient to inherit the convergence guarantees of InEKF and IS.
- domain assumption LiDAR odometry and GPS provide position measurements with zero-mean Gaussian noise.
- domain assumption The robot's contacts have zero velocity during stance, with slip events handled by a contact estimator and slip rejection [6].
Cite this review
Pith. "Pith review of Multi-Sensor Fusion for Quadruped Robot State Estimation using Invariant Filtering and Smoothing." pith.science (2026). https://pith.science/paper/PCNUA7CL
@misc{pith2026250420615,
author = {Pith},
title = {Pith review of: Multi-Sensor Fusion for Quadruped Robot State Estimation using Invariant Filtering and Smoothing},
year = {2026},
howpublished = {\url{https://pith.science/paper/PCNUA7CL}},
note = {Machine review of arXiv:2504.20615}
}
read the original abstract
This letter introduces two multi-sensor state estimation frameworks for quadruped robots, built on the Invariant Extended Kalman Filter (InEKF) and Invariant Smoother (IS). The proposed methods, named E-InEKF and E-IS, fuse kinematics, IMU, LiDAR, and GPS data to mitigate position drift, particularly along the z-axis, a common issue in proprioceptive-based approaches. We derived observation models that satisfy group-affine properties to integrate LiDAR odometry and GPS into InEKF and IS. LiDAR odometry is incorporated using Iterative Closest Point (ICP) registration on a parallel thread, preserving the computational efficiency of proprioceptive-based state estimation. We evaluate E-InEKF and E-IS with and without exteroceptive sensors, benchmarking them against LiDAR-based odometry methods in indoor and outdoor experiments using the KAIST HOUND2 robot. Our methods achieve lower Relative Position Errors (RPE) and significantly reduce Absolute Trajectory Error (ATE), with improvements of up to 28% indoors and 40% outdoors compared to LIO-SAM and FAST-LIO2. Additionally, we compare E-InEKF and E-IS in terms of computational efficiency and accuracy.
Figures
Figures from the paper (3 more)
Reference graph
Works this paper leans on
-
[7]
The Invariant Extended Kalman Filter as a Stable Observer,
A. Barrau and S. Bonnabel, “The Invariant Extended Kalman Filter as a Stable Observer,” IEEE Trans. Autom. Control , vol. 62, no. 4, pp. 1797–1812, 2017. TABLE III: ATE of the proposed E-InEKF and E-IS with different WS Proprioceptive only Proposed (LiDAR) Proposed (LiDAR + GPS) Filter 6.340664 m 1.683859 m 0.171617 m Smoother (WS:1) 6.331560 m 1.833524 m...
work page 2017
-
[12]
Invariant smoother for legged robot state estimation with dynamic contact event information,
Z. Yoon, J.-H. Kim, and H.-W. Park, “Invariant smoother for legged robot state estimation with dynamic contact event information,” IEEE Trans. Robot., vol. 40, pp. 193–212, 2024
work page 2024
-
[1]
LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing and mapping,
T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in 2020 IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS). IEEE, 2020, pp. 5135–5142
work page 2020
-
[2]
FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter,
W. Xu and F. Zhang, “FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter,” IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 3317–3324, 2021
work page 2021
-
[3]
FAST-LIO2: Fast direct LiDAR-inertial odometry,
W. Xu, Y . Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct LiDAR-inertial odometry,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2053– 2073, 2022
work page 2022
-
[4]
State estimation for legged robots: consistent fusion of leg kinematics and IMU,
M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring, C. D. Remy, and R. Siegwart, “State estimation for legged robots: consistent fusion of leg kinematics and IMU,” Robotics, vol. 17, pp. 17–24, Jul. 2013, DOI: 10.15607/RSS.2012.VIII.003
-
[5]
State estimation for legged robots on unstable and slippery terrain,
M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoepflinger, and R. Siegwart, “State estimation for legged robots on unstable and slippery terrain,” in 2013 IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS) , 2013, pp. 6058–6064
work page 2013
-
[6]
Legged robot state estimation with dynamic contact event information,
J.-H. Kim, S. Hong, G. Ji, S. Jeon, J. Hwangbo, J.-H. Oh, and H.-W. Park, “Legged robot state estimation with dynamic contact event information,” IEEE Robot. Autom. Lett. , vol. 6, no. 4, pp. 6733–6740, 2021
work page 2021
Show all 30 references
-
[8]
Contact-aided invariant extended Kalman filtering for robot state estimation,
R. Hartley, M. Ghaffari, R. M. Eustice, and J. W. Grizzle, “Contact-aided invariant extended Kalman filtering for robot state estimation,” Int. J. Robot. Res., vol. 39, no. 4, pp. 402–430, 2020
2020
-
[9]
Legged robot state estimation using invariant Kalman filtering and learned contact events,
T.-Y . Lin, R. Zhang, J. Yu, and M. Ghaffari, “Legged robot state estimation using invariant Kalman filtering and learned contact events,” arXiv preprint arXiv:2106.15713 , 2021
2021 arXiv
-
[10]
Proprioceptive state estimation for quadruped robots using invariant kalman filtering and scale-variant robust cost functions,
H. M. S. Santana, J. C. V . Soares, Y . Nistic `o, M. A. Meggiolaro, and C. Semini, “Proprioceptive state estimation for quadruped robots using invariant kalman filtering and scale-variant robust cost functions,” 2024 IEEE-RAS Int. Conf. Humanoid Robots , pp. 213–220, 2024
2024
-
[11]
Invariant smoothing on Lie groups,
P. Chauchat, A. Barrau, and S. Bonnabel, “Invariant smoothing on Lie groups,” in 2018 IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS) , 2018, pp. 1703–1710
2018
-
[13]
Pronto: A multi- sensor state estimator for legged robots in real-world scenarios,
M. Camurri, M. Ramezani, S. Nobili, and M. Fallon, “Pronto: A multi- sensor state estimator for legged robots in real-world scenarios,” Front. Robot. AI, vol. 7, 2020, DOI: 10.3389/frobt.2020.00068
2020
-
[14]
MUSE: A real-time multi-sensor state estimator for quadruped robots,
Y . Nistic`o, J. C. V . Soares, L. Amatucci, G. Fink, and C. Semini, “MUSE: A real-time multi-sensor state estimator for quadruped robots,” IEEE Robot. Autom. Lett. , pp. 1–8, 2025
2025
-
[15]
STEP: State estimator for legged robots using a preintegrated foot velocity factor,
Y . Kim, B. Yu, E. M. Lee, J. Kim, H. Park, and H. Myung, “STEP: State estimator for legged robots using a preintegrated foot velocity factor,” IEEE Robot. Autom. Lett. , vol. 7, no. 2, pp. 4456–4463, 2022
2022
-
[16]
VILENS: Visual, inertial, LiDAR, and leg odometry for all-terrain legged robots,
D. Wisth, M. Camurri, and M. Fallon, “VILENS: Visual, inertial, LiDAR, and leg odometry for all-terrain legged robots,” IEEE Trans. Robot., 2022
2022
-
[17]
Cerberus: Low-drift visual-inertial-leg odometry for agile locomotion,
S. Yang, Z. Zhang, Z. Fu, and Z. Manchester, “Cerberus: Low-drift visual-inertial-leg odometry for agile locomotion,” in 2023 IEEE Int. Conf. Robot. Autom. (ICRA) , 2023, pp. 4193–4199
2023
-
[18]
Leg-KILO: Robust kinematic-inertial-LiDAR odometry for dynamic legged robots,
G. Ou, D. Li, and H. Li, “Leg-KILO: Robust kinematic-inertial-LiDAR odometry for dynamic legged robots,” IEEE Robot. Autom. Lett. , vol. 9, no. 10, pp. 8194–8201, 2024
2024
-
[19]
Imu data processing for inertial aided navigation: A recurrent neural network based approach,
M. Zhang, M. Zhang, Y . Chen, and M. Li, “Imu data processing for inertial aided navigation: A recurrent neural network based approach,” in 2021 IEEE Int. Conf. Robot. Autom. (ICRA) . IEEE, 2021, pp. 3992– 3998
2021
-
[20]
Kalmannet: Neural network aided kalman filtering for partially known dynamics,
G. Revach, N. Shlezinger, X. Ni, A. L. Escoriza, R. J. Van Sloun, and Y . C. Eldar, “Kalmannet: Neural network aided kalman filtering for partially known dynamics,” IEEE Trans. Signal Process. , vol. 70, pp. 1532–1547, 2022
2022
-
[21]
Legged robot state estimation with invariant extended kalman filter using neural measurement network,
D. Youm, H. Oh, S. Choi, H. Kim, and J. Hwangbo, “Legged robot state estimation with invariant extended kalman filter using neural measurement network,” arXiv e-prints, pp. arXiv–2402, 2024
2024
-
[22]
Optistate: State estimation of legged robots using gated networks with transformer-based vision and kalman filtering,
A. Schperberg, Y . Tanaka, S. Mowlavi, F. Xu, B. Balaji, and D. Hong, “Optistate: State estimation of legged robots using gated networks with transformer-based vision and kalman filtering,” in 2024 IEEE Int. Conf. Robot. Autom. (ICRA) . IEEE, 2024, pp. 6314–6320
2024
-
[23]
State estimation transformers for agile legged locomotion,
C. Yu, Y . Yang, T. Liu, Y . You, M. Zhou, and D. Xiang, “State estimation transformers for agile legged locomotion,” in 2024 IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS) . IEEE, 2024, pp. 6810–6817
2024
-
[24]
Diter++: Diverse terrain and multi-modal dataset for multi-robot slam in multi-session environments,
J. Kim, H. Kim, S. Jeong, Y . Shin, and Y . Cho, “Diter++: Diverse terrain and multi-modal dataset for multi-robot slam in multi-session environments,” arXiv preprint arXiv:2412.05839 , 2024
2024 arXiv
-
[25]
Legged robot state estimation in slippery environments using invariant extended Kalman filter with velocity update,
S. Teng, M. W. Mueller, and K. Sreenath, “Legged robot state estimation in slippery environments using invariant extended Kalman filter with velocity update,” in 2021 IEEE Int. Conf. Robot. Autom. (ICRA) , 2021, pp. 3104–3110
2021
-
[26]
Invariant filtering for legged humanoid locomotion on a dynamic rigid surface,
Y . Gao, C. Yuan, and Y . Gu, “Invariant filtering for legged humanoid locomotion on a dynamic rigid surface,” IEEE/ASME Trans. Mechatron., vol. 27, no. 4, pp. 1900–1909, 2022
1900
-
[27]
KISS-ICP: In Defense of Point-to-Point ICP – Simple, Accurate, and Robust Registration If Done the Right Way,
I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, and C. Stachniss, “KISS-ICP: In Defense of Point-to-Point ICP – Simple, Accurate, and Robust Registration If Done the Right Way,” IEEE Robot. Autom. Lett., vol. 8, no. 2, pp. 1029–1036, 2023
2023
-
[28]
Design of KAIST HOUND, a quadruped robot platform for fast and efficient locomotion with mixed- integer nonlinear optimization of a gear train,
Y .-H. Shin, S. Hong, S. Woo, J. Choe, H. Son, G. Kim, J.-H. Kim, K. Lee, J. Hwangbo, and H.-W. Park, “Design of KAIST HOUND, a quadruped robot platform for fast and efficient locomotion with mixed- integer nonlinear optimization of a gear train,” in 2022 Int. Conf. Robot. Aut...
2022
-
[29]
A micro Lie theory for state estimation in robotics,
J. Sola, J. Deray, and D. Atchuthan, “A micro Lie theory for state estimation in robotics,” arXiv preprint arXiv:1812.01537 , 2018
2018 arXiv
-
[30]
Sensorless robot collision detection and hybrid force/motion control,
A. De Luca and R. Mattone, “Sensorless robot collision detection and hybrid force/motion control,” in Proceedings of the 2005 IEEE Int. Conf. Robot. Autom. (ICRA) . IEEE, 2005, pp. 999–1004
2005
Reviewed August 16, 2026 · model on record in the stance chip above.
Discussion (0). Continue with ORCID to comment.