1 Introduction↩︎

State estimation for serial rigid manipulators requires geometric consistency and stochastic rigour simultaneously: the configuration of each link is an element of the Lie group \(SE(3)\) [1], yet the classical Kalman filter operates in a flat vector space. Precise closed-loop control depends on accurate knowledge of the body-fixed twist and pose of each link, estimated from noisy IMU and joint encoder data in real time. The nonlinear geometry of \(SE(3)\) invalidates coordinate-based linearisation at large angles, and the heterogeneous physical origins of IMU and process noise require a treatment that most existing filter formulations do not provide.

Euler-angle and quaternion EKFs are well established [2], [3] but introduce unavoidable deficiencies: gimbal lock, normalisation constraints, and a linearisation consistency error that does not vanish at convergence [4], [5]. Geometric approaches have been pursued, including multiplicative quaternion filters [6], [7], dual-quaternion formulations [8], motor-algebra EKFs [9], and Lie-group signal processing [10], [11], but these address single bodies and do not propagate uncertainty modularly across chains.

Filters on Lie groups place geometric intuition within a rigorous stochastic framework. Probability densities on Lie groups and kinematic state estimation via the exponential map were developed in [12], [13]. The invariant EKF (IEKF) [4], [14] exploits a group-affine condition under which linearised error dynamics are autonomous, so the Riccati equation governs the true covariance. The IEKF has since been extended to matrix Lie groups [15], iterated and equivariant variants [16], [17], resilient distributed estimation [18], nonholonomic systems [19], and unscented formulations [20], with applications in legged odometry [21], inertial navigation [22], [23], and parallel mechanisms [24]. Despite this breadth, extension to serial multibody chains (where link states couple through kinematic constraints and uncertainty must propagate modularly) has received limited attention. Existing work treats either a single body or the full chain as a monolithic state, the latter incurring cubic update cost in link count.

A further gap concerns IMU noise. Gyroscope noise enters the rotational twist channel directly; accelerometer noise does not, since the accelerometer observes the time derivative of translational velocity. Recovery by integration introduces a sample-interval scaling on the effective noise covariance absent in the gyroscope channel, and gyroscope noise propagates through the Coriolis term creating a state-dependent process noise contribution that existing geometric filters do not address.

This paper makes four contributions to geometric stochastic estimation for serial rigid manipulators: a physically separated IMU noise model with state-dependent Coriolis noise (C1); a modular chain of per-link IEKFs with linear total cost via Adjoint covariance propagation (C2); derivation of the complete IEKF including the mixed pose-twist observation Jacobian (C3); and a chained exponential ultimate boundedness certificate via a Lie algebra Lyapunov function (C4).

The paper is organised as follows. Section 2 presents the system model and sensor measurement models. Section 3 establishes the geometric error state and linearized dynamics. Section 5 derives the linearized Jacobian and characterises all noise sources. Section 6 derives the predicted covariance and multibody synthesis. Section 7 derives the Kalman update with geometric retraction. Section 8 establishes exponential ultimate boundedness. Section 9 presents numerical results. Appendices 11–13 collect supporting proofs and derivations for the theoretical developments.

2 Geometric Preliminaries↩︎

A serial rigid manipulator with \(n\) revolute-jointed rigid links is considered. Each link \(i\) carries a body-fixed frame \({}^{i}\!\mathcal{F}\); its configuration \(g_i \in SE(3)\) and body-fixed twist \(\mathbf{V}_i = [\boldsymbol{\omega}_i^\top, \mathbf{v}_i^\top]^\top \in \mathfrak{se}(3)\cong\mathbb{R}^6\) satisfy \(\dot{g}_i = g_i\mathbf{V}_i\) [25], [26]. The standard geometric objects are collected here for reference:

\[\begin{align} g_i &= \begin{bmatrix} \mathbf{R}_{oi} & \mathbf{p}_i \\ \mathbf{0}^\top & 1 \end{bmatrix} \in SE(3), \tag{1}\\[4pt] \mathbf{Ad}_{ba} &= \begin{bmatrix} \mathbf{R}_{ba} & \mathbf{0} \\ [{}^{b}\mathbf{r}_a]^\times\mathbf{R}_{ba} & \mathbf{R}_{ba} \end{bmatrix} \in \mathbb{R}^{6\times6}, \tag{2}\\[4pt] \mathrm{ad}_{\mathbf{V}} &= \begin{bmatrix} [\boldsymbol{\omega}]^\times & \mathbf{0} \\ [\mathbf{v}]^\times & [\boldsymbol{\omega}]^\times \end{bmatrix} \in \mathbb{R}^{6\times6}, \tag{3} \end{align}\]

where \(\mathbf{Ad}_{ba}\) transforms twists from frame \(a\) to frame \(b\), with \(\mathbf{R}_{ba}\in SO(3)\) the rotation matrix and \({}^{b}\mathbf{r}_a\in\mathbb{R}^3\) the position of frame \(a\) expressed in frame \(b\); \(\mathrm{ad}_{\mathbf{V}}\) is the Lie bracket matrix on \(\mathfrak{se}(3)\); and \([\cdot]^\times\) denotes the skew-symmetric matrix. Filter estimates are denoted \(\hat{(\cdot)}\) throughout. The twist propagation across joint \(i\) is

\[\begin{align} \mathbf{V}_i = \mathbf{Ad}_{i(i-1)}\mathbf{V}_{i-1} + [\hat{\mathbf{z}}_i^\top,\mathbf{0}^\top]^\top\dot{q}_i, \label{eq:Vprop} \end{align}\tag{4}\]

where \(\hat{\mathbf{z}}_i\) is the joint axis and \(\dot{q}_i\) the joint rate. Wrenches \(\boldsymbol{\mathcal{W}}_i \in \mathfrak{se}^*(3)\) pair with twists via the natural power product. The Newton–Euler equation for link \(i\) is [26], [27]

\[\begin{align} \mathbf{M}_i\dot{\mathbf{V}}_i + \mathrm{ad}_{\mathbf{V}_i}^\top\mathbf{M}_i\mathbf{V}_i + \boldsymbol{\mathcal{W}}_{J,i} = \boldsymbol{\mathcal{W}}_i, \label{eq:newton95euler} \end{align}\tag{5}\]

where the spatial inertia \(\mathbf{M}_i = \bigl[\begin{smallmatrix}\mathbf{I}_{b_i} & -m_i[\mathbf{r}_{c_i}]^\times \\ m_i[\mathbf{r}_{c_i}]^\times & m_i\mathbf{I}_3\end{smallmatrix}\bigr]\succ 0\), with \(m_i\) the link mass, \(\mathbf{I}_{b_i}\) the rotational inertia in \({}^{i}\!\mathcal{F}\), \(\mathbf{r}_{c_i}\) the centre-of-mass position, \(\boldsymbol{\mathcal{W}}_{J,i}\) the joint constraint wrench, and \(\boldsymbol{\mathcal{W}}_i\) the applied control wrench.

3 Stochastic Dynamics and Observation Model↩︎

3.1 Process Noise: Disturbance Wrench↩︎

Unmodeled dynamics, friction, and payload uncertainty are represented as a disturbance wrench \(\mathbf{W}_i^w(t) \in \mathfrak{se}^*(3)\) acting additively on the Newton–Euler equation 5 :

\[\begin{align} \mathbf{M}_i\dot{\mathbf{V}}_i + \mathrm{ad}_{\mathbf{V}_i}^\top\mathbf{M}_i\mathbf{V}_i + \boldsymbol{\mathcal{W}}_{J,i} = \boldsymbol{\mathcal{W}}_i + \mathbf{W}_i^w(t), \label{eq:stoch95eom} \end{align}\tag{6}\]

modelled as zero-mean continuous-time white noise: \(\mathbb{E}[\mathbf{W}_i^w(t)(\mathbf{W}_i^w(s))^\top] = \mathbf{Q}_{c,i}^w\,\delta(t-s)\), \(\mathbf{Q}_{c,i}^w\succ 0\). Inverting \(\mathbf{M}_i\) identifies the drift \(\mathbf{f}_i^V(g_i,\mathbf{V}_i) = \mathbf{M}_i^{-1}(\boldsymbol{\mathcal{W}}_i - \mathrm{ad}_{\mathbf{V}_i}^\top\mathbf{M}_i\mathbf{V}_i - \boldsymbol{\mathcal{W}}_{J,i})\) and the noise input matrix \(\mathbf{G}_i^w = \mathbf{M}_i^{-1}\).

3.2 Measurement Model↩︎

IMU. The gyroscope measures body-fixed angular velocity directly: \[\begin{align} \mathbf{y}_i^{\mathrm{gyro}} = \boldsymbol{\omega}_i + \boldsymbol{\eta}_{\omega,i}, \qquad \boldsymbol{\eta}_{\omega,i} \sim \mathcal{N}(\mathbf{0},\,\mathbf{N}_{\omega,i}), \label{eq:gyro95meas} \end{align}\tag{7}\] where \(\mathbf{N}_{\omega,i}\in\mathbb{R}^{3\times3}\) is from the IMU datasheet. The accelerometer measures specific force in the body frame; applying the transport theorem gives: \[\begin{align} \mathbf{y}_i^{\mathrm{acc}} = \dot{\mathbf{v}}_i + [\boldsymbol{\omega}_i]^\times\mathbf{v}_i - \mathbf{R}_{oi}^\top\mathbf{g} + \boldsymbol{\eta}_{a,i}, \qquad \boldsymbol{\eta}_{a,i} \sim \mathcal{N}(\mathbf{0},\,\mathbf{N}_{a,i}), \label{eq:acc95meas} \end{align}\tag{8}\] where \(\mathbf{g}\in\mathbb{R}^3\) is the inertial gravity vector and \(\mathbf{N}_{a,i}\in\mathbb{R}^{3\times3}\) is the accelerometer noise covariance. The accelerometer observes \(\dot{\mathbf{v}}_i\), not \(\mathbf{v}_i\) directly. Integrating over \([t_k, t_{k+1}]\) after gravity compensation and stacking with the gyroscope yields the full twist measurement: \[\begin{align} \mathbf{y}_i^{\mathrm{IMU}} = \mathbf{V}_i(t_{k+1}) + \boldsymbol{\eta}_i, \qquad \boldsymbol{\eta}_i \sim \mathcal{N}(\mathbf{0},\,\mathbf{N}_i^{\mathrm{IMU}}), \label{eq:imu95meas} \end{align}\tag{9}\] with the stacked covariance: \[\begin{align} \mathbf{N}_i^{\mathrm{IMU}} = \begin{bmatrix} \mathbf{N}_{\omega,i} & \mathbf{0} \\ \mathbf{0} & \mathbf{N}_{a,i}\Delta t \end{bmatrix} \in \mathbb{R}^{6\times6}. \label{eq:Nimu} \end{align}\tag{10}\] The \(\Delta t\) factor on the accelerometer block arises from integrating the continuous spectral density \(\mathbf{N}_{a,i}\) over one sample interval — exactly analogous to the continuous-to-discrete conversion of process noise. This asymmetry between the gyroscope and accelerometer blocks is a defining feature of the noise model.

Joint encoder. The revolute encoder on joint \(i\) measures: \[\begin{align} y_i^{\mathrm{enc}} = q_i + n_i^{\mathrm{enc}}, \qquad n_i^{\mathrm{enc}} \sim \mathcal{N}(0,\,\sigma_{\mathrm{enc},i}^2), \label{eq:enc95meas} \end{align}\tag{11}\] where \(\sigma_{\mathrm{enc},i}^2\) is the encoder quantization variance. The encoder constrains the rotational DOF of \(g_i\) along \(\hat{\mathbf{z}}_i\), provides \(\mathbf{Ad}_{i(i-1)}\) for twist propagation, and supplies \(\dot{\hat{q}}_i\) for the relative joint twist.

3.3 Complete SDE and Observation Triple↩︎

The filter state for link \(i\) is \((g_i, \mathbf{V}_i) \in SE(3)\times\mathbb{R}^6\). The Kalman machinery operates on the Lie algebra error vector \(\tilde{\boldsymbol{\xi}}_i \in \mathbb{R}^{12}\) defined in Section 4. The plant SDE is [12], [28]: \[\begin{align} dg_i &= g_i\mathbf{V}_i\,dt, \tag{12}\\[4pt] d\mathbf{V}_i &= \mathbf{f}_i^V(g_i,\mathbf{V}_i)\,dt + \mathbf{G}_i^w\,d\mathbf{W}_i^w, \quad d\mathbf{W}_i^w \sim \mathcal{N}(\mathbf{0},\mathbf{Q}_{c,i}^w\,dt). \tag{13} \end{align}\] The pose equation has no direct Wiener increment; \(g_i\) is driven stochastically only through \(\mathbf{V}_i\). The stacked discrete-time measurement is: \[\begin{align} \mathbf{y}_i = \begin{bmatrix} \mathbf{y}_i^{\mathrm{IMU}} \\ y_i^{\mathrm{enc}} \end{bmatrix} = \begin{bmatrix} \mathbf{V}_i \\ q_i(g_i) \end{bmatrix} + \boldsymbol{\nu}_i, \quad \boldsymbol{\nu}_i \sim \mathcal{N}(\mathbf{0},\mathbf{N}_i^{\mathrm{obs}}), \label{eq:meas95full} \end{align}\tag{14}\] where \(q_i(g_i)\) is the joint angle extracted from \(g_i\) and: \[\begin{align} \mathbf{N}_i^{\mathrm{obs}} = \mathrm{diag}\!\left( \mathbf{N}_{\omega,i},\; \mathbf{N}_{a,i}\Delta t,\; \sigma_{\mathrm{enc},i}^2 \right) \in \mathbb{R}^{7\times7}. \label{eq:Nobs} \end{align}\tag{15}\] The observation function \(h_i(g_i,\mathbf{V}_i) = [\mathbf{V}_i^\top, q_i(g_i)]^\top\) is linear in \(\mathbf{V}_i\) and nonlinear in \(g_i\); its linearisation with respect to the Lie algebra error gives \(\mathbf{H}_{\mathrm{obs},i}\in\mathbb{R}^{7\times12}\), derived in Section 7.1. All stochasticity in the plant is contained in \(\mathbf{Q}_{c,i}^w\) (process) and \(\mathbf{N}_i^{\mathrm{obs}}\) (measurement). The inter-link twist coupling via 4 is deterministic in the plant equations; the associated uncertainty propagation through the Adjoint map enters the predicted covariance and is analysed in Section 6.

4 Geometric Error Dynamics↩︎

The filter state \((g_i, \mathbf{V}_i)\) lives on a manifold. The IEKF maintains a strict separation between the estimate \((\hat{g}_i, \hat{\mathbf{V}}_i) \in SE(3)\times\mathbb{R}^6\), the Lie algebra error vector \(\tilde{\boldsymbol{\xi}}_i \in \mathbb{R}^{12}\) on which all Kalman algebra operates, and the error covariance \(\mathbf{P}_i \in \mathbb{R}^{12\times12}\). The Kalman gain \(\mathbf{K}_i\in\mathbb{R}^{12\times7}\), derived in Section 7 after the noise analysis of Sections 5–6, maps the innovation \(\boldsymbol{\nu}_i = \mathbf{y}_i - h_i(\hat{g}_i^-,\hat{\mathbf{V}}_i^-)\in\mathbb{R}^7\) (measurement minus prediction) to a correction vector \(\delta\tilde{\boldsymbol{\xi}}_i = \mathbf{K}_i\boldsymbol{\nu}_i = [\delta\boldsymbol{\xi}_i^{g\top}, \delta\mathbf{V}_i^\top]^\top \in\mathbb{R}^{12}\), applied via geometric retraction:

\[\begin{align} \hat{g}_i^+ &= \hat{g}_i^- \cdot \exp\!\bigl([\delta\boldsymbol{\xi}_i^g]^\wedge\bigr), \tag{16}\\[4pt] \hat{\mathbf{V}}_i^+ &= \hat{\mathbf{V}}_i^- + \delta\mathbf{V}_i, \tag{17} \end{align}\]

where \(\exp:\mathfrak{se}(3)\to SE(3)\) keeps \(\hat{g}_i^+\) exactly on the manifold without normalisation.

The left-invariant pose error, twist error, and full error state are:

\[\begin{align} \tilde{g}_i &\triangleq \hat{g}_i^{-1} g_i \in SE(3), \tag{18}\\[4pt] \tilde{\boldsymbol{\xi}}_i^g &= \log(\tilde{g}_i)^\vee \in \mathbb{R}^6, \tag{19}\\[4pt] \tilde{\mathbf{V}}_i &= \mathbf{V}_i - \hat{\mathbf{V}}_i \in \mathbb{R}^6, \tag{20}\\[4pt] \tilde{\boldsymbol{\xi}}_i &= \bigl[\tilde{\boldsymbol{\xi}}_i^{g\top},\, \tilde{\mathbf{V}}_i^\top\bigr]^\top \in \mathbb{R}^{12}. \tag{21} \end{align}\]

The closed-form logarithmic map is given in Appendix 13. The filter minimises \(\mathbf{P}_i = \mathbb{E}[\tilde{\boldsymbol{\xi}}_i \tilde{\boldsymbol{\xi}}_i^\top]\) via a linearized SDE for \(\tilde{\boldsymbol{\xi}}_i\):

\[\begin{align} d\tilde{\boldsymbol{\xi}}_i = \mathbf{F}_{c,i}\tilde{\boldsymbol{\xi}}_i\,dt + \tilde{\mathbf{G}}_i^w\,d\mathbf{W}_i^w + \tilde{\mathbf{G}}_i^\eta\,d\boldsymbol{\zeta}_i, \label{eq:error95sde95preview} \end{align}\tag{22}\]

where \(\mathbf{F}_{c,i}\in\mathbb{R}^{12\times12}\) is the linearized error Jacobian, \(\tilde{\mathbf{G}}_i^w, \tilde{\mathbf{G}}_i^\eta\in\mathbb{R}^{12\times6}\) are noise input matrices, and \(d\boldsymbol{\zeta}_i\) collects the combined IMU and upstream velocity noise (defined in Section 6.1 after the noise analysis). Section 5 derives all terms from four independent noise sources: (i) wrench disturbance \(\mathbf{W}_i^w\) entering through \(\mathbf{M}_i^{-1}\); (ii) IMU noise \(\boldsymbol{\eta}_i\) propagating through the Coriolis linearization; (iii) upstream velocity error from link \(i-1\) through the Adjoint map; and (iv) encoder noise and upstream pose error, acting as a discrete shift on the pose block. Sources (i)–(iii) enter continuously; source (iv) acts on the initial condition.

5 Group-Affine Error Model and Linearized Dynamics↩︎

5.1 Group-Affine Property↩︎

The pose kinematics \(\dot{g}_i = g_i\mathbf{V}_i\) satisfy the group-affine property [4]: the left-invariant error \(\tilde{g}_i = \hat{g}_i^{-1}g_i\) evolves autonomously as

\[\begin{align} \dot{\tilde{g}}_i = \tilde{g}_i\mathbf{V}_i - \hat{\mathbf{V}}_i\tilde{g}_i, \label{eq:gerror95dot2} \end{align}\tag{23}\]

independently of \(\hat{g}_i\). The proof is in Appendix 11. Consequently the IEKF linearization is exact for the pose kinematics and the Riccati equation governs the true error covariance.

5.2 Linearized Error Jacobian \(\mathbf{F}_{c,i}\)↩︎

Writing \(\tilde{g}_i = \exp([\tilde{\boldsymbol{\xi}}_i^g]^\wedge)\) and truncating to first order via the derivative of the matrix exponential [29] gives \(\dot{\tilde{g}}_i \approx \tilde{g}_i[\dot{\tilde{\boldsymbol{\xi}}}_i^g]^\wedge\). Substituting into 23 , left-multiplying by \(\tilde{g}_i^{-1}\), and expanding the conjugation to first order in \(\tilde{\boldsymbol{\xi}}_i^g\) (see Appendix 12 for the full derivation) yields

\[\begin{align} \dot{\tilde{\boldsymbol{\xi}}}_i^g \approx -\mathrm{ad}_{\hat{\mathbf{V}}_i}\tilde{\boldsymbol{\xi}}_i^g + \tilde{\mathbf{V}}_i. \label{eq:xi95dot95pose} \end{align}\tag{24}\]

For the velocity error, substituting \(\mathbf{V}_i = \hat{\mathbf{V}}_i + \tilde{\mathbf{V}}_i\) into \(\mathrm{ad}_{\mathbf{V}_i}^\top\mathbf{M}_i\mathbf{V}_i\) and retaining first-order terms requires expressing the perturbation \(\delta\mathbf{H}_i = \mathrm{ad}_{\tilde{\mathbf{V}}_i}^\top\mathbf{M}_i\hat{\mathbf{V}}_i + \mathrm{ad}_{\hat{\mathbf{V}}_i}^\top\mathbf{M}_i\tilde{\mathbf{V}}_i\) as a linear map in \(\tilde{\mathbf{V}}_i\). Applying the identity \(\mathrm{ad}_\mathbf{a}^\top\mathbf{b} = \mathrm{ad}^*_\mathbf{b}\,\mathbf{a}\), where

\[\begin{align} \mathrm{ad}^*_\mathbf{b} \triangleq \begin{bmatrix} [\mathbf{p}]^\times & \mathbf{0} \\ [\mathbf{q}]^\times & [\mathbf{p}]^\times \end{bmatrix}, \quad \mathbf{b} = [\mathbf{p}^\top,\mathbf{q}^\top]^\top, \label{eq:adstar95def} \end{align}\tag{25}\]

gives \(\delta\mathbf{H}_i = \mathbf{C}_i\tilde{\mathbf{V}}_i\) with the linearized Coriolis matrix

\[\begin{align} \mathbf{C}_i = \mathrm{ad}_{\hat{\mathbf{V}}_i}^\top\mathbf{M}_i + \mathrm{ad}^*_{\mathbf{M}_i\hat{\mathbf{V}}_i} \in \mathbb{R}^{6\times6}, \label{eq:Ci95def} \end{align}\tag{26}\]

so that the velocity error evolves as \(\dot{\tilde{\mathbf{V}}}_i = -\mathbf{M}_i^{-1}\mathbf{C}_i\tilde{\mathbf{V}}_i + \mathbf{M}_i^{-1}\mathbf{W}_i^w\). Stacking with 24 :

\[\begin{align} \mathbf{F}_{c,i} = \begin{bmatrix} -\mathrm{ad}_{\hat{\mathbf{V}}_i} & \mathbf{I}_6 \\ \mathbf{0} & -\mathbf{M}_i^{-1}\mathbf{C}_i \end{bmatrix} \in \mathbb{R}^{12\times12}. \label{eq:Fci} \end{align}\tag{27}\]

\(\mathbf{F}_{c,i}\) depends on \(\hat{\mathbf{V}}_i\) but not on \(\tilde{\boldsymbol{\xi}}_i\), confirming the autonomous error dynamics property.

6 Itô Analysis, Covariance Propagation, and Modular Synthesis↩︎

6.1 Noise Input Matrices and Total Velocity Uncertainty↩︎

With \(\mathbf{F}_{c,i}\) established, the remaining terms of the prediction error SDE 22 are the noise input matrices. The wrench channel gives directly \(\tilde{\mathbf{G}}_i^w = [\mathbf{0};\mathbf{M}_i^{-1}]\) with spectral density \(\mathbf{Q}_{c,i}^w\). Two further channels enter through the Coriolis linearization \(\mathbf{C}_i\):

IMU Coriolis noise. The predicted twist uses the noisy IMU measurement, contributing \(\tilde{\mathbf{V}}_i^{\mathrm{IMU}} \approx \boldsymbol{\eta}_i\) to the velocity error. This propagates through \(\mathbf{C}_i\) into the velocity channel with covariance \(\mathbf{C}_i\mathbf{N}_i^{\mathrm{IMU}}\mathbf{C}_i^\top\).

Upstream velocity error. The predicted twist of link \(i\) is built from the posterior of link \(i-1\): \[\begin{align} \tilde{\mathbf{V}}_i^{\mathrm{up}} = \mathbf{Ad}_{i(i-1)}\,\tilde{\mathbf{V}}_{i-1}^+, \label{eq:V95up} \end{align}\tag{28}\] where \(\mathbf{P}_{i-1,VV}^+\in\mathbb{R}^{6\times6}\) is the velocity block of the link \(i-1\) posterior covariance. Both channels enter the same Coriolis path and are independent (past measurements of link \(i-1\) vs.current IMU noise of link \(i\)), so their covariances add to form the total effective twist uncertainty: \[\begin{align} \mathbf{N}_i^{\mathrm{total}} \triangleq \mathbf{Ad}_{i(i-1)}\mathbf{P}_{i-1,VV}^+ \mathbf{Ad}_{i(i-1)}^\top + \mathbf{N}_i^{\mathrm{IMU}}. \label{eq:Ntotal} \end{align}\tag{29}\] For link 1, \(\mathbf{P}_{0,VV}^+=\mathbf{0}\) and \(\mathbf{N}_1^{\mathrm{total}}=\mathbf{N}_1^{\mathrm{IMU}}\). The complete noise input matrices are: \[\begin{align} \tilde{\mathbf{G}}_i^w &= \begin{bmatrix}\mathbf{0}\\\mathbf{M}_i^{-1}\end{bmatrix} \in \mathbb{R}^{12\times6}, \tag{30}\\[4pt] \tilde{\mathbf{G}}_i^\eta &= \begin{bmatrix}\mathbf{0}\\-\mathbf{M}_i^{-1}\mathbf{C}_i\end{bmatrix} \in \mathbb{R}^{12\times6}, \tag{31} \end{align}\] with the combined diffusion matrix: \[\begin{align} \mathbf{D}_i &= \tilde{\mathbf{G}}_i^w\mathbf{Q}_{c,i}^w(\tilde{\mathbf{G}}_i^w)^\top + \tilde{\mathbf{G}}_i^\eta\mathbf{N}_i^{\mathrm{total}} (\tilde{\mathbf{G}}_i^\eta)^\top \notag\\ &= \begin{bmatrix} \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{M}_i^{-1}(\mathbf{Q}_{c,i}^w + \mathbf{C}_i\mathbf{N}_i^{\mathrm{total}}\mathbf{C}_i^\top) \mathbf{M}_i^{-\top} \end{bmatrix}. \label{eq:Di} \end{align}\tag{32}\] The pose block of both \(\tilde{\mathbf{G}}_i^w\) and \(\tilde{\mathbf{G}}_i^\eta\) is zero: noise drives \(\tilde{\boldsymbol{\xi}}_i^g\) only through the \((1,2)\) coupling \(\mathbf{I}_6\) in \(\mathbf{F}_{c,i}\).

6.2 Itô Covariance ODE and Discretization↩︎

Applying the Itô formula to \(\tilde{\boldsymbol{\xi}}_i\tilde{\boldsymbol{\xi}}_i^\top\), using independence of wrench and IMU/upstream noise (physically separate sensors and links), gives the Lyapunov ODE: \[\begin{align} \dot{\mathbf{P}}_i = \mathbf{F}_{c,i}\mathbf{P}_i + \mathbf{P}_i\mathbf{F}_{c,i}^\top + \mathbf{D}_i, \label{eq:lyapunov95ct2} \end{align}\tag{33}\] where \(\mathbf{P}_i = \mathbb{E}[\tilde{\boldsymbol{\xi}}_i \tilde{\boldsymbol{\xi}}_i^\top]\in\mathbb{R}^{12\times12}\). Integrating 33 exactly over \([t_k,t_{k+1}]\) with \(\mathbf{F}_{c,i}\) frozen gives the discrete prediction error: \[\begin{align} \tilde{\boldsymbol{\xi}}_{i,k+1}^- = \boldsymbol{\Phi}_{i,k}\tilde{\boldsymbol{\xi}}_{i,k} + \mathbf{w}_{i,k}, \quad \mathbf{w}_{i,k}\sim \mathcal{N}(\mathbf{0},\tilde{\mathbf{Q}}_{d,i}^{\mathrm{eff}}), \label{eq:xi95discrete} \end{align}\tag{34}\] and the covariance recursion: \[\begin{align} \mathbf{P}_i(t_{k+1}) = \boldsymbol{\Phi}_i\mathbf{P}_i^+\boldsymbol{\Phi}_i^\top + \tilde{\mathbf{Q}}_{d,i}^{\mathrm{eff}}, \label{eq:P95exact} \end{align}\tag{35}\] where \[\begin{align} \boldsymbol{\Phi}_i \approx \mathbf{I}_{12} + \mathbf{F}_{c,i}\Delta t + \tfrac{1}{2}(\mathbf{F}_{c,i}\Delta t)^2, \label{eq:Phi95i} \end{align}\tag{36}\] and the effective discrete noise covariance: \[\begin{align} \tilde{\mathbf{Q}}_{d,i}^{\mathrm{eff}} = \int_0^{\Delta t} e^{\mathbf{F}_{c,i}s}\mathbf{D}_i e^{\mathbf{F}_{c,i}^\top s}ds \approx \mathbf{D}_i\Delta t + O(\Delta t^2), \label{eq:Qd95embed} \end{align}\tag{37}\] computed exactly via the Van Loan method [30].

6.3 Modular Prediction Synthesis↩︎

Equation 35 accounts for three of four noise sources through \(\mathbf{D}_i\): wrench disturbance, IMU noise, and upstream velocity error. The fourth — encoder noise and upstream pose error — acts on the initial condition of \(\tilde{\boldsymbol{\xi}}_i^g\) and is derived below.

Pose prediction. From the product-of-exponentials chain [25]: \[\begin{align} \hat{g}_i^- = \hat{g}_{i-1}^+ \cdot g_{(i-1)i}^{\mathrm{ref}} \cdot \exp\!\bigl(y_i^{\mathrm{enc}}[\hat{\mathbf{z}}_i]^\wedge\bigr), \label{eq:g95pred95chain} \end{align}\tag{38}\] where \(g_{(i-1)i}^{\mathrm{ref}}\in SE(3)\) is the reference configuration at \(q_i=0\). The left-invariant pose error \(\tilde{g}_i^- = (\hat{g}_i^-)^{-1}g_i\) expands as: \[\begin{align} \tilde{g}_i^- = h^{-1}\tilde{g}_{i-1}^+\,h\cdot \exp(n_i^{\mathrm{enc}}[\hat{\mathbf{z}}_i]^\wedge), \label{eq:gerror95conjugate} \end{align}\tag{39}\] where \(h \triangleq g_{(i-1)i}^{\mathrm{ref}}\cdot \exp(y_i^{\mathrm{enc}}[\hat{\mathbf{z}}_i]^\wedge)\) is the nominal relative transform from frame \(i-1\) to frame \(i\) at the measured encoder angle, satisfying \(\mathbf{Ad}_{h^{-1}} = \mathbf{Ad}_{i(i-1)}\). Applying the first-order Adjoint conjugation identity and then the BCH formula: \[\begin{align} \tilde{\boldsymbol{\xi}}_i^{g,-} \approx \mathbf{Ad}_{i(i-1)}\,\tilde{\boldsymbol{\xi}}_{i-1}^{g,+} + \mathbf{j}_{\mathrm{enc},i}\,n_i^{\mathrm{enc}}, \label{eq:xi95pose95prop} \end{align}\tag{40}\] where \(\mathbf{j}_{\mathrm{enc},i} = [\hat{\mathbf{z}}_i^\top,\mathbf{0}^\top]^\top\in\mathbb{R}^6\) is the unit joint screw. Taking the covariance of 40 gives the upstream pose term: \[\begin{align} \mathbf{P}_i^{-,\mathrm{pose}} = \begin{bmatrix} \mathbf{Ad}_{i(i-1)}\mathbf{P}_{i-1,gg}^+ \mathbf{Ad}_{i(i-1)}^\top + \sigma_{\mathrm{enc},i}^2\mathbf{j}_{\mathrm{enc},i} \mathbf{j}_{\mathrm{enc},i}^\top & \mathbf{0} \\ \mathbf{0} & \mathbf{0} \end{bmatrix}, \label{eq:P95upstream95gg} \end{align}\tag{41}\] where \(\mathbf{P}_{i-1,gg}^+\in\mathbb{R}^{6\times6}\) is the pose block of the link \(i-1\) posterior covariance.

Complete predicted covariance. Adding 41 to 35 : \[\begin{align} \mathbf{P}_i^- = \boldsymbol{\Phi}_i\mathbf{P}_i^+\boldsymbol{\Phi}_i^\top + \tilde{\mathbf{Q}}_{d,i}^{\mathrm{eff}} + \mathbf{P}_i^{-,\mathrm{pose}}. \label{eq:P95pred95expanded} \end{align}\tag{42}\] All four noise sources are now accounted for. For the base link \(\mathbf{P}_1^{-,\mathrm{pose}}=\mathbf{0}\), recovering the single-body result.

Twist prediction. \[\begin{align} \hat{\mathbf{V}}_i^- = \mathbf{Ad}_{i(i-1)}\hat{\mathbf{V}}_{i-1}^+ + [\hat{\mathbf{z}}_i^\top,\mathbf{0}^\top]^\top \dot{\hat{q}}_i. \label{eq:V95pred95chain} \end{align}\tag{43}\] The velocity prediction error \(\tilde{\mathbf{V}}_i^- = \mathbf{Ad}_{i(i-1)}\tilde{\mathbf{V}}_{i-1}^+\) is absorbed into \(\mathbf{N}_i^{\mathrm{total}}\) 29 .

Information flow. The predicted covariance \(\mathbf{P}_i^-\) requires from link \(i-1\) only \((\hat{g}_{i-1}^+, \hat{\mathbf{V}}_{i-1}^+, \mathbf{P}_{i-1}^+)\): \[\begin{align} (\hat{g}_{i-1}^+,\hat{\mathbf{V}}_{i-1}^+,\mathbf{P}_{i-1}^+) \;\to\; \mathbf{P}_i^- \;\to\; \mathbf{K}_i \;\to\; (\hat{g}_i^+,\hat{\mathbf{V}}_i^+,\mathbf{P}_i^+). \label{eq:info95flow} \end{align}\tag{44}\]

7 Kalman Update↩︎

7.1 Observation Matrix, Gain, and Innovation↩︎

The observation function \(h_i(g_i,\mathbf{V}_i) = [\mathbf{V}_i^\top, q_i(g_i)]^\top\in\mathbb{R}^7\) is linear in \(\mathbf{V}_i\) and nonlinear in \(g_i\). Linearizing with respect to \(\tilde{\boldsymbol{\xi}}_i\): the IMU rows give \([\mathbf{0}_{6\times6},\mathbf{I}_6]\) directly; the encoder row contributes the unit joint screw \(\mathbf{j}_{q,i}^\top = [\hat{\mathbf{z}}_i^\top, \mathbf{0}_{1\times3}]\), which selects the rotational pose error along \(\hat{\mathbf{z}}_i\). The full observation matrix, innovation, innovation covariance, and Kalman gain are:

\[\begin{align} \mathbf{H}_{\mathrm{obs},i} &= \begin{bmatrix} \mathbf{0}_{6\times6} & \mathbf{I}_6 \\ \mathbf{j}_{q,i}^\top & \mathbf{0}_{1\times6} \end{bmatrix} \in \mathbb{R}^{7\times12}, \tag{45}\\[4pt] \boldsymbol{\nu}_i &= \mathbf{y}_i - h_i(\hat{g}_i^-, \hat{\mathbf{V}}_i^-) \in \mathbb{R}^7, \tag{46}\\[4pt] \mathbf{S}_i &= \mathbf{H}_{\mathrm{obs},i}\mathbf{P}_i^- \mathbf{H}_{\mathrm{obs},i}^\top + \mathbf{N}_i^{\mathrm{obs}}, \tag{47}\\[4pt] \mathbf{K}_i &= \mathbf{P}_i^-\mathbf{H}_{\mathrm{obs},i}^\top \mathbf{S}_i^{-1}. \tag{48} \end{align}\]

Note that \(\mathbf{N}_i^{\mathrm{obs}}\in\mathbb{R}^{7\times7}\) from 15 combines IMU and encoder noise, and is distinct from \(\mathbf{N}_i^{\mathrm{IMU}}\in\mathbb{R}^{6\times6}\) (gyro/accel only, used in the Coriolis channel) and \(\mathbf{N}_i^{\mathrm{total}}\in\mathbb{R}^{6\times6}\) (combined twist uncertainty in \(\mathbf{D}_i\)).

7.2 State Update and Covariance↩︎

The correction \(\delta\tilde{\boldsymbol{\xi}}_i = \mathbf{K}_i\boldsymbol{\nu}_i\) is applied via the geometric retraction 16 –17 . The posterior covariance uses the Joseph form for numerical stability:

\[\begin{align} \mathbf{L}_i &= \mathbf{I}_{12} - \mathbf{K}_i\mathbf{H}_{\mathrm{obs},i}, \tag{49}\\[4pt] \mathbf{P}_i^+ &= \mathbf{L}_i\mathbf{P}_i^-\mathbf{L}_i^\top + \mathbf{K}_i\mathbf{N}_i^{\mathrm{obs}}\mathbf{K}_i^\top. \tag{50} \end{align}\]

The posterior \((\hat{g}_i^+,\hat{\mathbf{V}}_i^+, \mathbf{P}_i^+)\) is passed as upstream input to link \(i+1\).

Complete Per-Link Algorithm↩︎

The algorithm is identical for every link \(i=1,\ldots,n\), with base initialisation \(g_0=\mathbf{I}_4\), \(\mathbf{V}_0=\mathbf{0}\), \(\mathbf{P}_0^+=\mathbf{0}\).

8 Stability and Boundedness Analysis↩︎

This section establishes exponential ultimate boundedness (EUB) in mean square of the per-link error \(\tilde{\boldsymbol{\xi}}_i\in\mathbb{R}^{12}\) and chains the results across the manipulator. The framework follows [31] adapted to the Lie group setting. A key correction to naive \(SE(3)\) stability arguments: the Adjoint map is not an isometry under the standard Euclidean inner product on \(\mathbb{R}^6\) (SE(3) admits no bi-invariant metric), so the chained bound must account for the geometry-dependent Adjoint amplification factor.

8.1 Closed-Loop Error Recursion↩︎

The discrete prediction error 34 is corrected by the Kalman update to give the closed-loop recursion. For the twist block the update is \(\tilde{\mathbf{V}}_{i,k+1}^+ = \tilde{\mathbf{V}}_{i,k+1}^- - \delta\mathbf{V}_{i,k+1}\). For the pose block, composing the true error with the geometric retraction 16 gives \(\tilde{g}_{i,k+1}^+ = \exp(-[\delta\boldsymbol{\xi}_{i,k+1}^g]^\wedge) \cdot\tilde{g}_{i,k+1}^-\), and taking the logarithm via the discrete BCH identity yields: \[\begin{align} \tilde{\boldsymbol{\xi}}_{i,k+1}^{g,+} = \tilde{\boldsymbol{\xi}}_{i,k+1}^{g,-} - \delta\boldsymbol{\xi}_{i,k+1}^g + \tfrac{1}{2}[\delta\boldsymbol{\xi}_{i,k+1}^g, \tilde{\boldsymbol{\xi}}_{i,k+1}^{g,-}] + \cdots \label{eq:BCH95expand} \end{align}\tag{51}\] The remainder is \(O(\|\tilde{\boldsymbol{\xi}}_{i,k}\|^2)\) since both \(\delta\boldsymbol{\xi}_{i,k+1}^g\) and \(\tilde{\boldsymbol{\xi}}_{i,k+1}^{g,-}\) are \(O(\|\tilde{\boldsymbol{\xi}}_{i,k}\|)\) via 34 . This \(O(\|\cdot\|^2)\) order is the key advantage of geometric retraction over a coordinate EKF, where the linearisation error enters at first order [4]. Substituting \(\tilde{\boldsymbol{\xi}}_{i,k+1}^- = \boldsymbol{\Phi}_{i,k}\tilde{\boldsymbol{\xi}}_{i,k} + \mathbf{w}_{i,k}\) and \(\delta\tilde{\boldsymbol{\xi}}_{i,k+1} = \mathbf{K}_{i,k}\mathbf{H}_{\mathrm{obs},i} \tilde{\boldsymbol{\xi}}_{i,k+1}^-\) gives: \[\begin{align} \tilde{\boldsymbol{\xi}}_{i,k+1} = \mathbf{A}_{i,k}\boldsymbol{\Phi}_{i,k} \tilde{\boldsymbol{\xi}}_{i,k} + \mathbf{A}_{i,k}\mathbf{w}_{i,k} + O(\|\tilde{\boldsymbol{\xi}}_{i,k}\|^2), \label{eq:error95closed95loop} \end{align}\tag{52}\] where \(\mathbf{A}_{i,k} = \mathbf{I}_{12} - \mathbf{K}_{i,k}\mathbf{H}_{\mathrm{obs},i}\) and \(\mathbf{w}_{i,k}\sim \mathcal{N}(\mathbf{0},\tilde{\mathbf{Q}}_{d,i}^{\mathrm{eff}})\).

Under these assumptions the Riccati equation has a unique bounded solution \(\mathbf{P}_i^\infty\succ0\) and there exist uniform bounds \(\underline{p}\mathbf{I}\preceq \mathbf{P}_{i,k}^-\preceq\overline{p}\mathbf{I}\) [31].

8.2 Per-Link Exponential Ultimate Boundedness↩︎

8.3 Adjoint Operator Norm↩︎

8.4 Chained Bound↩︎

9 Results and discussion↩︎

The theoretical contributions of Sections 3–8 are validated through a numerical simulation of a 3-DOF two-link rigid serial manipulator executing large-amplitude three-dimensional motion. The study isolates the geometric estimation advantage of the proposed IEKF by controlled comparison against a coordinate EKF and the raw measurement baseline under exact ground truth, providing quantitative RMSE and NEES analysis under controlled noise statistics. Filter parameters are set directly from the physical noise characterisation of Section 3: gyroscope and accelerometer covariances from sensor datasheets, encoder variance from the encoder resolution, and the wrench disturbance spectral density tuned by innovation consistency [32].

Three-Dimensional Two-Link Rigid Chain↩︎

The simulated plant is a two-link rigid serial manipulator in three-dimensional motion. Link 1 connects to the fixed base via a two-DOF revolute joint (rotations about body-fixed \(y\)- and \(z\)-axes); Link 2 connects to Link 1 via a single-DOF revolute joint about \(z\). The system is a 3-DOF chain on \(SE(3)\); all six twist components of both links are nonzero via Adjoint propagation 4 .

Mechanical parameters: \(m_1=2.0\), \(m_2=1.5\) [kg]; \(l_1=0.50\), \(l_2=0.40\) [m]; \(\mathbf{I}_{b_i}=\mathrm{diag}(10^{-3},\,m_il_i^2/12,\, m_il_i^2/12)\) [kg\(\cdot\)m\(^2\)]. Noise parameters follow Section 3: \(\sigma_\omega=0.05\) [rad/s], \(\sigma_a=0.20\) [m/s\(^2\)], \(\sigma_{\mathrm{enc}}=0.5^\circ\), \(\mathbf{Q}_{c,1}^w=\mathrm{diag}(0.08^2\mathbf{I}_3, 0.03^2\mathbf{I}_3)\), \(\mathbf{Q}_{c,2}^w=\mathrm{diag}(0.10^2\mathbf{I}_3, 0.04^2\mathbf{I}_3)\) [N\(^2\)m\(^2\)/Hz]. The plant is integrated at \(\Delta t=5\times10^{-3}\) [s] for \(t_f=15\) [s] by fourth-order Runge–Kutta applied to 5 .

The desired joint angles are prescribed as \[\begin{align} \theta_{1y,d} &= -p_{e,d,z}/L,\quad \theta_{1z,d} = p_{e,d,y}\,(l_1/L)/l_1,\notag\\ \theta_{2,d}^{\mathrm{abs}} &= p_{e,d,y}/L + A_{y,2}\sin(\omega_{d,2}t)\,\rho(t)/l_2, \label{eq:sim95traj} \end{align}\tag{60}\] where \(L=l_1+l_2=0.90\) [m], \(A_{y,2}=0.10\) [m], \(\omega_{d,2}=1.5\) [rad/s], and \(p_{e,d,y}\), \(p_{e,d,z}\) follow the Lissajous pattern with ramp-up \(\rho(t)=1-e^{-t/1.5}\): \[\begin{align} p_{e,d,y}(t) &= 0.25\sin(0.8t)\,\rho(t),\quad p_{e,d,z}(t) = 0.15\cos(0.8t)\,\rho(t). \notag \end{align}\] The end-effector traces a 3D curve through the forward kinematics of these joint angles; the \(x\)-coordinate varies naturally with joint configuration rather than being fixed.

Three methods are compared. The proposed IEKF implements Algorithm 1 with the full noise model of Section 3, including the accelerometer \(\Delta t\) scaling 10 and Coriolis coupling in \(\mathbf{D}_i\) 32 ; per-link error state dimension 12, total chain dimension 24. The coordinate EKF (CEKF) operates on the flat joint-space state \([\theta_{1y},\theta_{1z},\theta_2^{\mathrm{abs}}, \dot{\theta}_{1y},\dot{\theta}_{1z},\dot{\theta}_2^{\mathrm{abs}}]^\top \in\mathbb{R}^6\) with linearised double-integrator dynamics and encoder-only measurements; it uses no IMU and no \(SE(3)\) structure, making it a weaker baseline by design. The no-filter baseline uses raw encoder readings directly.

Body-frame twist estimation↩︎

Figure 2 shows true and IEKF-estimated body-frame twists for both links. All nine plotted components are nonzero, confirming genuine three-dimensional motion. The IEKF tracks the true twist closely on all channels; the \(2\sigma\) covariance band from 42 consistently envelops the true value. The band is wider for Link 2, whose covariance incorporates upstream estimation uncertainty through \(\mathbf{N}_2^{\mathrm{total}}\) 29 .

Estimation errors↩︎

Figure 3 shows absolute joint-angle estimation error for all three methods, as well as tracking response for the endpoint trajectory. The IEKF achieves the smallest error on all joints. The advantage is most pronounced on \(\theta_2\): Adjoint-propagated upstream covariance and IMU fusion provide velocity information unavailable to the encoder-only CEKF.

RMSE comparison↩︎

Table 2 reports RMSE for all methods and both scenarios. The IEKF achieves \(33\%\) reduction relative to the raw encoder baseline and \(24\%\) relative to the CEKF on all joints. The CEKF RMSE grows from S1 to S2 due to coordinate linearisation error at larger amplitudes, while IEKF RMSE remains stable — reflecting amplitude-invariance of the \(SE(3)\) geometric error model 22 .

NEES analysis↩︎

Figure 5 shows the NEES of the IEKF for Link 2, computed on the \(n_f=7\) dimensions of the measurement vector [32]. The mean NEES of \(3.5\) lies within the \(95\%\) chi-squared acceptance band \([1.7,\,16.0]\) throughout, confirming statistical consistency of \(\mathbf{P}_2^-\) 42 . The value below the ideal of \(7\) indicates a conservative filter, a deliberate consequence of inflating \(\mathbf{Q}_{c,i}^w\) for robustness.

10 Conclusion↩︎

An invariant extended Kalman filter for state estimation of serial rigid manipulators with an arbitrary number of links has been presented, grounded in a geometric stochastic framework on \(SE(3)\). The group-affine property of the pose kinematics ensures that the linearised error dynamics are autonomous and the Riccati equation governs the true error covariance. A physically separated noise model treats the gyroscope and accelerometer independently: the accelerometer yields an effective velocity measurement covariance scaling with \(\Delta t\), and a state-dependent Coriolis noise term captures gyroscope noise propagating through the nonlinear Newton–Euler dynamics, vanishing at rest and growing with twist magnitude. The filter is structured as a modular chain of per-link IEKFs with \(O(n)\) cost, exploiting cross-link noise independence and Adjoint covariance propagation. Exponential ultimate boundedness in mean square is established via a Lie algebra Lyapunov function, with per-link bounds chained through the Adjoint operator norm to yield a modular stability certificate scalable to arbitrary chain length.

Several directions remain open. On the theoretical side, explicitly characterising the observability Gramian 53 in terms of manipulator geometry and motion trajectory, and establishing conditions under which Assumption 2 is guaranteed, constitutes a natural continuation. The explicit \(\alpha_i\) formula 56 and the geometry-dependent residual ball in 59 provide quantitative design targets for sensor placement and sampling rate. On the implementation side, augmenting the per-link state with IMU bias vectors extends the SDE 13 within the same framework, and deployment on a physical platform requires empirical validation of the NEES bounds against experimental ground truth via innovation consistency tuning of \(\mathbf{Q}_{c,i}^w\). More broadly, the SE(3)-structured error coordinates and Adjoint covariance propagation derived here provide a geometrically consistent representation of manipulator state uncertainty that is well suited to serve as a physical prior or supervision signal for learning-based state estimators, where maintaining Lie group consistency under learned corrections remains an open problem.

11 Proof of the Group-Affine Property↩︎

12 Derivation of the Linearized Pose Error Dynamics↩︎

This appendix derives 24 in full. Writing \(\tilde{g}_i = \exp([\tilde{\boldsymbol{\xi}}_i^g]^\wedge)\) and applying the first-order approximation \(\dot{\tilde{g}}_i \approx \tilde{g}_i[\dot{\tilde{\boldsymbol{\xi}}}_i^g]^\wedge\) (from [29] truncated at \(\|\tilde{\boldsymbol{\xi}}_i^g\|\)), substituting into 23 and left-multiplying by \(\tilde{g}_i^{-1}\): \[\begin{align} [\dot{\tilde{\boldsymbol{\xi}}}_i^g]^\wedge = [\mathbf{V}_i]^\wedge - \tilde{g}_i^{-1}[\hat{\mathbf{V}}_i]^\wedge\tilde{g}_i. \label{eq:lin95step295app} \end{align}\tag{62}\] Expanding the conjugation to first order using \(\tilde{g}_i = \mathbf{I} + [\tilde{\boldsymbol{\xi}}_i^g]^\wedge + O(\|\tilde{\boldsymbol{\xi}}_i^g\|^2)\): \[\begin{align} \tilde{g}_i^{-1}[\hat{\mathbf{V}}_i]^\wedge\tilde{g}_i = [\hat{\mathbf{V}}_i]^\wedge + [[\hat{\mathbf{V}}_i]^\wedge,[\tilde{\boldsymbol{\xi}}_i^g]^\wedge] + O(\|\tilde{\boldsymbol{\xi}}_i^g\|^2), \label{eq:conj95expand95app} \end{align}\tag{63}\] where \([\cdot,\cdot]\) is the matrix commutator (Lie bracket on \(\mathfrak{se}(3)\)). Substituting 63 into 62 and using \([\mathbf{V}_i]^\wedge - [\hat{\mathbf{V}}_i]^\wedge = [\tilde{\mathbf{V}}_i]^\wedge\): \[\begin{align} [\dot{\tilde{\boldsymbol{\xi}}}_i^g]^\wedge = [\tilde{\mathbf{V}}_i]^\wedge - [[\hat{\mathbf{V}}_i]^\wedge,[\tilde{\boldsymbol{\xi}}_i^g]^\wedge] + O(\|\tilde{\boldsymbol{\xi}}_i^g\|^2). \label{eq:lin95step395app} \end{align}\tag{64}\] Applying the vee operator \((\cdot)^\vee\) to both sides of 64 and using the definition of the Lie bracket matrix 3 , \([[\hat{\mathbf{V}}_i]^\wedge, [\tilde{\boldsymbol{\xi}}_i^g]^\wedge]^\vee = \mathrm{ad}_{\hat{\mathbf{V}}_i}\tilde{\boldsymbol{\xi}}_i^g\), yields 24  [4].0◻

13 \(SE(3)\) Logarithmic Map↩︎

The left-invariant error \(\tilde{g}_i = \hat{g}_i^{-1}g_i\) has components [25]: \[\begin{align} \tilde{\mathbf{R}}_i = \hat{\mathbf{R}}_i^\top\mathbf{R}_i, \qquad \tilde{\mathbf{p}}_i = \hat{\mathbf{R}}_i^\top (\mathbf{p}_i - \hat{\mathbf{p}}_i). \label{eq:gerror95components} \end{align}\tag{65}\]

\(SO(3)\) logarithm. The rotation angle and axis are: \[\begin{align} \theta_i &= \cos^{-1}\!\left(\tfrac{\mathrm{tr}(\tilde{\mathbf{R}}_i)-1}{2} \right)\in[0,\pi), \tag{66}\\[4pt] \boldsymbol{\phi}_i &= \frac{\theta_i}{2\sin\theta_i} \begin{bmatrix} \tilde{R}_{32}-\tilde{R}_{23}\\ \tilde{R}_{13}-\tilde{R}_{31}\\ \tilde{R}_{21}-\tilde{R}_{12} \end{bmatrix}, \quad \theta_i\neq0, \tag{67} \end{align}\] with \(\boldsymbol{\phi}_i=\mathbf{0}\) at \(\theta_i=0\). At \(\theta_i\to\pi\) the skew-part formula is singular; the axis must be recovered from the symmetric part of \(\tilde{\mathbf{R}}_i\) as the eigenvector for eigenvalue \(+1\).

\(SE(3)\) logarithm. The translation component requires the left Jacobian inverse [12]: \[\begin{align} \mathbf{J}_l^{-1}(\boldsymbol{\phi}) = \frac{\theta/2}{\tan(\theta/2)}\mathbf{I}_3 + \!\left(1-\frac{\theta/2}{\tan(\theta/2)}\right) \frac{\boldsymbol{\phi}\boldsymbol{\phi}^\top}{\theta^2} - \frac{1}{2}[\boldsymbol{\phi}]^\times. \label{eq:Jlinv} \end{align}\tag{68}\] The complete six-dimensional pose error vector is: \[\begin{align} \tilde{\boldsymbol{\xi}}_i^g = \log(\tilde{g}_i)^\vee = \begin{bmatrix} \boldsymbol{\phi}_i \\[4pt] \mathbf{J}_l^{-1}(\boldsymbol{\phi}_i)\,\tilde{\mathbf{p}}_i \end{bmatrix} \in \mathbb{R}^6. \label{eq:xi95pose95expanded} \end{align}\tag{69}\] At \(\theta_i=0\): \(\mathbf{J}_l^{-1}=\mathbf{I}_3\) and \(\tilde{\boldsymbol{\xi}}_i^g\approx [\mathbf{0}^\top,\tilde{\mathbf{p}}_i^\top]^\top\), recovering the linearized expression used in first-order analyses.

References↩︎

---

1. This work was supported by the Research Council of Finland under the project “Nonlinear PDE-model-based control of flexible manipulators” (Grant No. 355664). (Corresponding author: S. Yaqubi.)↩︎

2. S. Yaqubi and J. Mattila are with the Department of Automation Technology and Mechanical Engineering, Tampere University, Korkeakoulunkatu 6, 33720 Tampere, Finland (e-mails: sadeq.yaqubi@tuni.fi, jouni.mattila@tuni.fi).↩︎

3. This document is an arXiv preprint posted for open access and citation purposes. It is under review and subject to revision.↩︎