The Multivariate Feedback Particle Filter for State-dependent Multiplicative Noises and Its Application to Attitude and Pose Estimations
Authors: Jingyao You, Xue Luo
Organizations: School of Mathematical Sciences, Beihang University, Beijing 102206, China · School of Mathematical Sciences and Key Laboratory of Mathematics, Informatics and Behavioral Semantics (LMIB), Beihang University, Beijing 100191, China
Nonlinear filtering with state-dependent multiplicative measurement noise poses a notable challenge for spacecraft attitude determination and robotic pose estimation. This work develops the FPF-DM, a decomposition scheme for feedback particle filters tailored to this family of filtering problems. Extending prior decomposition techniques for additive noise and polynomial measurement models, the proposed method resolves the key difficulty that state-dependent multiplicative noise breaks the polynomial assumption and prevents Hermite spectral backward recursion for the weighted Poisson's equation. By measure transformation and importance reweighting, we incorporate the inverse measurement covariance into particle weights and rescale the feedback gain to restore the polynomial-form Poisson's equation, with the decomposition generalized to non-uniformly weighted mixtures for closed-form gain computation. This general FPF-DM is instantiated on quaternion and dual-quaternion manifolds for attitude tracking and point-cloud registration. Simulations on 4D attitude tracking and 8D point-cloud registration verify that the instantiated filters using only 10 particles achieve superior accuracy and computational performance relative to baseline filters.
Figures & tables
Xtn+1i
=Xtni+g(Xtni)Δt+σ(Xtni)ΔBtni
+K(Xtni)ΔItni.
Algorithm 1 Multivariate FPF-DM for Multiplicative Observation Noise
Fig. 1: Tracking trajectories of the four unit quaternion components q0,q1,q2,q3 over time for one single run
Fig. 2: Geodesic attitude estimation error in SO(3) versus time of a single run
Fig. 3: Comparison of steady-state geodesic attitude RMSE over 100 MC runs
Method
Np
Att. RMSE ( ∘ )
Quat. RMSE
Avg Time (s)
QFPF-DM
10
0.5740
0.00501
0.5819
QFPF-CG
50
2.1875
0.01909
0.3243
QPF
200
1.7093
0.01492
2.6796
QEKF
—
3.4369
0.02999
0.0983
TABLE I: 4D quaternion attitude tracking: averaged RMSE errors and runtime over 100 MC runs ( T=100 s, Nt=10000 )
Method
Np
q0
q1
q2
q3
tx (mm)
ty (mm)
tz (mm)
RMS (mm)
Time (s)
Actual Pose
—
0.866
0.500
0.000
0.000
50.000
42.000
20.000
—
—
DQFPF-DM
10
0.867
0.499
−0.000
−0.000
49.989
42.065
20.097
0.1732
0.4015
DQFPF-CG
50
0.866
0.500
−0.003
0.000
49.866
42.010
20.236
0.3751
0.1788
DQPF
200
0.866
0.500
−0.000
−0.000
49.815
41.991
20.252
0.3144
0.9991
DQKF
—
0.870
0.493
−0.003
−0.006
58.787
46.473
20.685
9.9548
0.0031
TABLE II: Estimated pose parameters, final registration RMS error, and execution time on the noisy Stanford bunny dataset
Fig. 4: Point cloud registration RMS error versus the number of sequential measurements on the noisy Stanford Bunny dataset
Fig. 5: Point cloud registration RMS errors over 100 MC runs: (a) all four methods and (b) zoomed-in view of three comparative methods: DQFPF-DM, DQFPF-CG, and DQPF
Method
Np
Avg RMS (mm)
Avg Time (s)
DQFPF-DM
10
0.2169
0.3342
DQFPF-CG
50
0.3999
0.1809
DQPF
200
0.5940
0.9991
DQKF
—
9.5210
0.0036
TABLE III: Averaged performance comparison for 8D point cloud registration over 100 Monte Carlo runs
Learning-based methods commonly treat state estimation in robotics as a sequence modeling problem. While this paradigm can be effective at maximizing end-to-end performance, models are often difficult to interpret and expensive to train, since training requires unrolling sequences of predictions in time. As an alternative to end-to-end trained state estimation, we propose a novel particle filtering algorithm in which models are trained from individual state transitions, fully exploiting the Markov property in robotic systems. In this framework, measurement models are learned implicitly by minimizing a denoising score matching objective. At inference, the learned denoiser is used alongside a (learned) dynamics model to approximately solve the Bayesian filtering equation at each time step, effectively guiding predicted states toward the data manifold informed by measurements. We evaluate the proposed method on challenging robotic state estimation tasks in simulation, demonstrating competitive performance compared to tuned end-to-end trained baselines. Importantly, our method offers the desirable composability of classical filtering algorithms, allowing prior information and external sensor models to be incorporated without retraining.
Lennart Röstel, Berthold Bäuml
Learning AI for Dextrous Robots Lab, Technical University of Munich, Germany.
Sequential inference estimates latent states from noisy and incomplete observations. Particle Filters (PFs), a class of Monte Carlo methods based on importance sampling, provide a flexible framework for this task, but often suffer from poor sample efficiency and unfavorable scaling with dimension, partly due to suboptimal proposal distributions. We address these challenges by integrating learned proposals into the PF framework. We introduce Neural Optimal Particle Filters (NOPFs), which learn an amortized approximation to the optimal proposal from offline simulated one-step conditioning tuples. The learned proposal is used as a drop-in replacement in standard PF updates, with samples corrected by standard importance weights so that the method asymptotically targets the same filtering distribution under standard support and density-evaluation assumptions. Across stochastic nonlinear benchmarks of varying inference complexity, NOPFs improve sample efficiency and distributional accuracy over standard PF baselines with modest computational overhead. The approach integrates data-driven proposal learning into classical inference without altering the underlying filtering objective.
This paper introduces the ensemble directional Kalman filter (EnDKF), an ensemble-based Kalman filtering approach for pose tracking that jointly estimates an object's position and attitude using ideas from directional statistics. The EnDKF integrates a unit-quaternion attitude representation to move beyond canonical Kalman filter mean and covariance assumptions that poorly capture directional uncertainty. Experiments on a synthetic constant-velocity constant-angular-velocity system and a digital-twin head-tracking scenario using the FoundationPose algorithm demonstrate a significant reduction in error as opposed to merely using measurements.
Tianlu Lu, Asif Sijan, Thomas Noh +2
Information and Computer Sciences Department at the University of Hawaii at Manoa · John A. Burns School of Medicine at the University of Hawaii at Manoa