Wrench-Based Bayesian Pose Estimation via Matrix--Fisher Gaussian Inference

Abstract

In this paper, a residual-safeguarded local Matrix Fisher--Gaussian (MFG) inference method is developed for wrench-based pose estimation on $\mathrm{SO}(3)\times\mathbb{R}^3$. The force/torque measurements are modeled by a quasi-static contact system in which the predicted wrench depends on the unknown object pose through an implicit equilibrium state. Since the resulting nonlinear likelihood is not globally conjugate to the coupled MFG family, a local Bayesian update is constructed by linearizing the reduced wrench residual and matching the induced Gauss--Newton posterior model to a coupled MFG distribution. It is shown that the reduced residual Jacobian has a Schur-complement form, and that the local quadratic posterior admits a closed-form MFG approximation matching the prescribed local first- and second-order posterior coefficients. The same sensitivity model yields a compensated rotational information score, which characterizes the weakest locally informative attitude direction after translational compensation. A residual-safeguarded recentering algorithm is further introduced to update the linearization point only through candidates that decrease the recomputed whitened wrench residual. In the tested sparse prior-mismatch regimes, the resulting estimator reduces residual merit and pose error relative to single-pass and local baseline variants, and controlled robot experiments provide a proof of concept under calibrated quasi-static conditions.