Appendix 1: Derivation and the application of the rotation error vector
For the additive error matrix, due to the orthogonality of both \( \hat{\varvec{R}} \) and \( \varvec{R} \), for small errors we have
$$ \varvec{I}_{3} = \varvec{RR}^{T} = \left( {\hat{\varvec{R}} - {\text{d}}\hat{\varvec{R}}} \right)\left( {\hat{\varvec{R}} - {\text{d}}\hat{\varvec{R}}} \right)^{T} = \hat{\varvec{R}}\hat{\varvec{R}}^{T} - {\text{d}}\hat{\varvec{R}}\hat{\varvec{R}}^{T} - \hat{\varvec{R}}{\text{d}}\hat{\varvec{R}}^{T} + \text{d}\hat{\varvec{R}}{\text{d}}\hat{\varvec{R}}^{T} \approx \varvec{I}_{3} - {\text{d}}\hat{\varvec{R}}\hat{\varvec{R}}^{T} - \hat{\varvec{R}}{\text{d}}\hat{\varvec{R}}^{T} $$
(43)
where the last approximation on the right is correct to the first order of \( {\text{d}}\hat{\varvec{R}} \). From (43), we have
$$ {\text{d}}\hat{\varvec{R}}\hat{\varvec{R}}^{T} \approx - \hat{\varvec{R}}{\text{d}}\hat{\varvec{R}}^{T} = - \left( {{\text{d}}\hat{\varvec{R}}\hat{\varvec{R}}^{T} } \right)^{T} $$
(44)
i.e., \( {\text{d}}\hat{\varvec{R}}\hat{\varvec{R}}^{T} \) is skew-symmetric or anti-symmetric. Any 3 × 3 skew-symmetric matrix is the cross-product matrix of an appropriate 3 × 1 vector (Dorst 2005). Let this vector be θ, we have
$$ \left[ {\varvec{\theta}\times } \right] = {\text{d}}\hat{\varvec{R}}\hat{\varvec{R}}^{T} $$
(45)
So we can easily have the following,
$$ \varvec{R} = \hat{\varvec{R}} - {\text{d}}\hat{\varvec{R}} = \left( {\varvec{I}_{3} - {\text{d}}\hat{\varvec{R}}\hat{\varvec{R}}^{T} } \right)\hat{\varvec{R}} = \left( {\varvec{I}_{3} - \left[ {\varvec{\theta}\times } \right]} \right)\hat{\varvec{R}} $$
(46)
From the definition of the multiplicative error, i.e., \( \varvec{R} =\updelta\hat{\varvec{R}}\hat{\varvec{R}} \) and (46), we can further have (18). Actually, the multiplicative error \( \updelta\hat{\varvec{R}} \) itself represents an error rotation. The coordinates in the old frame is transformed to that in the estimated new frame through \( \hat{\varvec{R}} \) and further transformed to that in the true new frame through \( \updelta\hat{\varvec{R}} \). For small errors, up to first-order terms, θ can be any of the following 3-dimension representations of the error rotation whose rotation matrix is \( \updelta\hat{\varvec{R}} \), i.e., the Euler angles, the rotation vector, two times the Gibbs vector, or four times the modified Rodrigues parameters. The error vector θ can be called a multiplicative error (vector) herein; the “multiplicative” is associated with the rotation composition rule, more specifically, the true rotation is composited in sequence of the estimated rotation represented by the estimated rotation matrix and the error rotation whose rotation matrix is constructed using θ. We will use θ to code the error in \( \hat{\varvec{R}} \). When using \( \hat{\varvec{R}} \) to transform coordinates of a vector, e.g., \( \hat{\varvec{y}} = \hat{\varvec{R}}\varvec{x} \), the errors in \( \hat{\varvec{R}} \) propagate as follows,
$$ {\text{d}}\hat{\varvec{y}} = \hat{\varvec{y}} - \varvec{y} = \hat{\varvec{R}}\varvec{x} - \varvec{Rx} = {\text{d}}\hat{\varvec{R}}\varvec{x} = \left[ {\varvec{\theta}\times } \right]\hat{\varvec{R}}\varvec{x} = - \left[ {\left( {\hat{\varvec{R}}\varvec{x}} \right) \times } \right]\varvec{\theta} $$
(47)
So given the mean and covariance of θ, say \( \bar{\varvec{\theta }} \) and \( \varvec{P}_{{\varvec{\theta \theta }}} \), we have the following,
$$ \begin{aligned} E\left[ {{\text{d}}\hat{\varvec{y}}} \right] & = \left[ {\left( {\hat{\varvec{R}}\varvec{x}} \right) \times } \right]\bar{\varvec{\theta }} \\ \text{cov}\left[ {{\text{d}}\hat{\varvec{y}}} \right] & = \left[ {\left( {\hat{\varvec{R}}\varvec{x}} \right) \times } \right]\varvec{P}_{{\varvec{\theta \theta }}} \left[ {\left( {\hat{\varvec{R}}\varvec{x}} \right) \times } \right]^{T} = - \left[ {\left( {\hat{\varvec{R}}\varvec{x}} \right) \times } \right]\varvec{P}_{{\varvec{\theta \theta }}} \left[ {\left( {\hat{\varvec{R}}\varvec{x}} \right) \times } \right] \\ \end{aligned} $$
(48)
This clearly shows the convenient application of the rotation error vector in the coordinate transformation.
Appendix 2: Expression of dh in terms of the measurement errors
From (5), we have the following,
$$ \begin{aligned} \text{d}\bar{\varvec{x}} = \frac{1}{m}\text{d}\varvec{X}{\mathbf{1}}_{m} = \frac{1}{m}\left( {{\mathbf{1}}_{m}^{T} \otimes \varvec{I}_{3} } \right)\varvec{\xi}\hfill \\ \text{d}\bar{\varvec{y}} = \frac{1}{m}\text{d}\varvec{Y}{\mathbf{1}}_{m} = \frac{1}{m}\left( {{\mathbf{1}}_{m}^{T} \otimes \varvec{I}_{3} } \right)\varvec{\zeta}\hfill \\ \end{aligned} $$
(49)
Let ρ = vec[E
x
], η = vec[E
y
], the following can be obtained from (7),
$$ \begin{aligned} {\text{d}}\varvec{\rho}& = {\text{vec}}\left[ {\text{d}\varvec{E}_{x} } \right] = {\text{vec}}\left[ {\text{d}\varvec{XK}} \right] = \left( {\varvec{K} \otimes \varvec{I}_{3} } \right)\varvec{\xi}\\ {\text{d}}\varvec{\eta}& = {\text{vec}}\left[ {\text{d}\varvec{E}_{y} } \right] = {\text{vec}}\left[ {\text{d}\varvec{YK}} \right] = \left( {\varvec{K} \otimes \varvec{I}_{3} } \right)\varvec{\zeta}\\ \end{aligned} $$
(50)
From (8), we have the following,
$$ \text{d}\varvec{H} = \text{d}\varvec{E}_{y} \varvec{E}_{x}^{T} + \varvec{E}_{y} \text{d}\varvec{E}_{x}^{T} $$
(51)
Then from (51) we have
$$ {\text{d}}\varvec{h} = {\text{vec}}\left[ {\text{d}\varvec{H}} \right] = \left( {\varvec{E}_{x} \otimes \varvec{I}_{3} } \right){\text{vec}}\left[ {{\text{d}}\varvec{E}_{y} } \right] + \left( {\varvec{I}_{3} \otimes \varvec{E}_{y} } \right){\text{vec}}\left[ {\text{d}\varvec{E}_{x}^{T} } \right] $$
(52)
Introduce the commutation matrix (Magnus and Neudecker 2007),
$$ \varvec{G} = \left[ {\begin{array}{*{20}c} {\varvec{z}_{1} } & {\varvec{z}_{4} } & \cdots & {\varvec{z}_{3m - 2} } &| & {\varvec{z}_{2} } & {\varvec{z}_{5} } & \cdots & {\varvec{z}_{3m - 1} } &| & {\varvec{z}_{3} } & {\varvec{z}_{6} } & \cdots & {\varvec{z}_{3m} } \\ \end{array} } \right]^{T} $$
(53)
where
$$ \varvec{I}_{3m} = \left[ {\begin{array}{*{20}c} {\varvec{z}_{1} } & {\varvec{z}_{2} } & \cdots & {\varvec{z}_{3m} } \\ \end{array} } \right] $$
(54)
we have the following,
$$ {\text{vec}}\left[ {\text{d}\varvec{E}_{x}^{T} } \right] = \varvec{G}{\text{vec}}\left[ {\text{d}\varvec{E}_{x} } \right] $$
(55)
Substitute (55) into (52) we obtain
$$ {\text{d}}\varvec{h} = \left( {\varvec{E}_{x} \otimes \varvec{I}_{3} } \right){\text{vec}}\left[ {{\text{d}}\varvec{E}_{y} } \right] + \left( {\varvec{I}_{3} \otimes \varvec{E}_{y} } \right)\varvec{G}{\text{vec}}\left[ {\text{d}\varvec{E}_{x} } \right] $$
(56)
Substituting (50) into (56), we finally have (22).
Appendix 3: Expression of θ in terms of dh
From (11), we have the following,
$$ {\text{d}}\hat{\varvec{R}} = {\text{d}}{\bar{\varvec{U}}\bar{\varvec{V}}}^{T} + \bar{\varvec{U}}{\text{d}}\bar{\varvec{V}}^{T} $$
(57)
So the following holds:
$$ \left[ {\varvec{\theta}\times } \right] = {\text{d}}\hat{\varvec{R}}\hat{\varvec{R}}^{T} = \left( {{\text{d}}{\bar{\varvec{U}}\bar{\varvec{V}}}^{T} + \bar{\varvec{U}}{\text{d}}\bar{\varvec{V}}^{T} } \right){\bar{\varvec{V}}\bar{\varvec{U}}}^{T} = \bar{\varvec{U}}\left( {\bar{\varvec{U}}^{T} {\text{d}}\bar{\varvec{U}} + {\text{d}}\bar{\varvec{V}}^{T} \bar{\varvec{V}}} \right)\bar{\varvec{U}}^{T} $$
(58)
In a similar manner as (43) through (45), we can define the following skew-symmetric matrices,
$$ \begin{aligned} \left[ {\varvec{u} \times } \right] & = \bar{\varvec{U}}^{T} {\text{d}}\bar{\varvec{U}} \\ \left[ {\varvec{v} \times } \right] & = {\text{d}}\bar{\varvec{V}}^{T} \bar{\varvec{V}} \\ \end{aligned} $$
(59)
So substituting (59) into (58), we obtain
$$ \left[ {\varvec{\theta}\times } \right] = \bar{\varvec{U}}\left( {\left[ {\varvec{u} \times } \right] + \left[ {\varvec{v} \times } \right]} \right)\bar{\varvec{U}}^{T} $$
(60)
The vector equivalence of (60) is as
$$ \varvec{\theta}= \bar{\varvec{U}}\left( {\varvec{u} + \varvec{v}} \right) $$
(61)
From (9) and (10), we get
$$ \varvec{H} = {\bar{\varvec{U}}\bar{\varvec{D}}\bar{\varvec{V}}}^{T} $$
(62)
with
$$ \bar{\varvec{D}} = \left[ {\begin{array}{*{20}c} {d_{1} } & 0 & 0 \\ 0 & {d_{2} } & 0 \\ 0 & 0 & {d_{3} \det \left[ \varvec{U} \right]\det \left[ \varvec{V} \right]} \\ \end{array} } \right] $$
(63)
Note that the relations 1/det[U] = det[U] and 1/det[V] = det[V] have been used in (63). The errors in (62) are as follows,
$$ {\text{d}}\varvec{H} = {\text{d}}{\bar{\varvec{U}}\bar{\varvec{D}}\bar{\varvec{V}}}^{T} + \bar{\varvec{U}}{\text{d}}{\bar{\varvec{D}}\bar{\varvec{V}}}^{T} + {\bar{\varvec{U}}\bar{\varvec{D}}}{\text{d}}\bar{\varvec{V}}^{T} $$
(64)
From (64) we have the following,
$$ \bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}} = \bar{\varvec{U}}^{T} {\text{d}}{\bar{\varvec{U}}\bar{\varvec{D}}} + {\text{d}}\bar{\varvec{D}} + \bar{\varvec{D}}{\text{d}}\bar{\varvec{V}}^{T} \bar{\varvec{V}} = \left[ {\varvec{u} \times } \right]\bar{\varvec{D}} + {\text{d}}\bar{\varvec{D}} + \bar{\varvec{D}}\left[ {\varvec{v} \times } \right] $$
(65)
Since \( \left( {\bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}}} \right)^{T} - \bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}} \) is skew-symmetric, let
$$ \left[ {\varvec{\omega}\times } \right] = \left( {\bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}}} \right)^{T} - \bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}} $$
(66)
Substitute (65) into (66), we have the following.
$$ \begin{aligned} \left[ {\varvec{\omega}\times } \right] & = \bar{\varvec{D}}\left[ {\varvec{u} \times } \right]^{T} + {\text{d}}\bar{\varvec{D}} + \left[ {\varvec{v} \times } \right]^{T} \bar{\varvec{D}} - \left[ {\varvec{u} \times } \right]\bar{\varvec{D}} - {\text{d}}\bar{\varvec{D}} - \bar{\varvec{D}}\left[ {\varvec{v} \times } \right] \\ & = - \bar{\varvec{D}}\left[ {\varvec{u} \times } \right] - \left[ {\varvec{v} \times } \right]\bar{\varvec{D}} - \left[ {\varvec{u} \times } \right]\bar{\varvec{D}} - \bar{\varvec{D}}\left[ {\varvec{v} \times } \right] \\ & = - \bar{\varvec{D}}\left( {\left[ {\varvec{u} \times } \right] + \left[ {\varvec{v} \times } \right]} \right) - \left( {\left[ {\varvec{u} \times } \right] + \left[ {\varvec{v} \times } \right]} \right)\bar{\varvec{D}} \\ \end{aligned} $$
(67)
The vector equivalence of (67) is as
$$ \varvec{u} + \varvec{v} = - \tilde{\varvec{D}}^{ - 1}\varvec{\omega} $$
(68)
with \( \tilde{\varvec{D}} \) defined in (24). From (61) and (68) we have
$$ \varvec{\theta}= \bar{\varvec{U}}\left( {\varvec{u} + \varvec{v}} \right) = - {\bar{\varvec{U}}\tilde{\varvec{D}}}^{ - 1}\varvec{\omega} $$
(69)
Let
$$ \varvec{I}_{3} = \left[ {\begin{array}{*{20}c} {\varvec{e}_{1} } & {\varvec{e}_{2} } & {\varvec{e}_{3} } \\ \end{array} } \right] $$
(70)
From (66), we have
$$ \begin{aligned}\varvec{\omega}& = \left[ {\begin{array}{*{20}c} {\varvec{e}_{3}^{T} \left[ {\varvec{\omega}\times } \right]\varvec{e}_{2} } \\ {\varvec{e}_{1}^{T} \left[ {\varvec{\omega}\times } \right]\varvec{e}_{3} } \\ {\varvec{e}_{2}^{T} \left[ {\varvec{\omega}\times } \right]\varvec{e}_{1} } \\ \end{array} } \right] \\ & = \left[ {\begin{array}{*{20}c} {\varvec{e}_{3}^{T} \left( {\bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}}} \right)^{T} \varvec{e}_{2} } \\ {\varvec{e}_{1}^{T} \left( {\bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}}} \right)^{T} \varvec{e}_{3} } \\ {\varvec{e}_{2}^{T} \left( {\bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}}} \right)^{T} \varvec{e}_{1} } \\ \end{array} } \right] - \left[ {\begin{array}{*{20}c} {\varvec{e}_{3}^{T} \bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}e}_{2} } \\ {\varvec{e}_{1}^{T} \bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}e}_{3} } \\ {\varvec{e}_{2}^{T} \bar{\varvec{U}}^{T} {\text{d}}\varvec{H\bar{V}e}_{1} } \\ \end{array} } \right] \\ \end{aligned} $$
(71)
Rearrange (71) in terms of dh as follows,
$$ \begin{aligned}\varvec{\omega}= \left[ {\begin{array}{*{20}c} {\left( {\varvec{e}_{3}^{T} \bar{\varvec{V}}^{T} } \right) \otimes \left( {\varvec{e}_{2}^{T} \bar{\varvec{U}}^{T} } \right)} \\ {\left( {\varvec{e}_{1}^{T} \bar{\varvec{V}}^{T} } \right) \otimes \left( {\varvec{e}_{3}^{T} \bar{\varvec{U}}^{T} } \right)} \\ {\left( {\varvec{e}_{2}^{T} \bar{\varvec{V}}^{T} } \right) \otimes \left( {\varvec{e}_{1}^{T} \bar{\varvec{U}}^{T} } \right)} \\ \end{array} } \right]{\text{d}}\varvec{h} - \left[ {\begin{array}{*{20}c} {\left( {\varvec{e}_{2}^{T} \bar{\varvec{V}}^{T} } \right) \otimes \left( {\varvec{e}_{3}^{T} \bar{\varvec{U}}^{T} } \right)} \\ {\left( {\varvec{e}_{3}^{T} \bar{\varvec{V}}^{T} } \right) \otimes \left( {\varvec{e}_{1}^{T} \bar{\varvec{U}}^{T} } \right)} \\ {\left( {\varvec{e}_{1}^{T} \bar{\varvec{V}}^{T} } \right) \otimes \left( {\varvec{e}_{2}^{T} \bar{\varvec{U}}^{T} } \right)} \\ \end{array} } \right]{\text{d}}\varvec{h} \hfill \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} = \varvec{S}{\text{d}}\varvec{h} \hfill \\ \end{aligned} $$
(72)
with
$$ \varvec{S} = \left[ {\begin{array}{*{20}c} {\left[ {\left( {\bar{\varvec{V}}\varvec{e}_{3} } \right) \otimes \left( {\bar{\varvec{U}}\varvec{e}_{2} } \right) - \left( {\bar{\varvec{V}}\varvec{e}_{2} } \right) \otimes \left( {\bar{\varvec{U}}\varvec{e}_{3} } \right)} \right]^{T} } \\ {\left[ {\left( {\bar{\varvec{V}}\varvec{e}_{1} } \right) \otimes \left( {\bar{\varvec{U}}\varvec{e}_{3} } \right) - \left( {\bar{\varvec{V}}\varvec{e}_{3} } \right) \otimes \left( {\bar{\varvec{U}}\varvec{e}_{1} } \right)} \right]^{T} } \\ {\left[ {\left( {\bar{\varvec{V}}\varvec{e}_{2} } \right) \otimes \left( {\bar{\varvec{U}}\varvec{e}_{1} } \right) - \left( {\bar{\varvec{V}}\varvec{e}_{1} } \right) \otimes \left( {\bar{\varvec{U}}\varvec{e}_{2} } \right)} \right]^{T} } \\ \end{array} } \right] $$
(73)
So finally from (69) and (72), we have (23).
Appendix 4: Expression of \( {\text{d}}\hat{c} \) in terms of the measurement errors
From (12) we have the following,
$$ {\text{d}}\hat{c} = \text{tr}\left[ {{\text{d}}\hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} } \right] + \text{tr}\left[ {\hat{\varvec{R}}{\text{d}}\varvec{E}_{x} \varvec{E}_{y}^{T} } \right] + \text{tr}\left[ {\hat{\varvec{R}}\varvec{E}_{x} {\text{d}}\varvec{E}_{y}^{T} } \right] $$
(74)
Equation (45) provides the following relation,
$$ \text{tr}\left[ {{\text{d}}\hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} } \right] = \text{tr}\left[ {\left[ {\varvec{\theta}\times } \right]\hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} } \right] $$
(75)
For any 3 × 3 matrix \( \varvec{C} = \left[ {\begin{array}{*{20}c} {c_{11} } & {c_{12} } & {c_{13} } \\ {c_{21} } & {c_{22} } & {c_{23} } \\ {c_{31} } & {c_{32} } & {c_{33} } \\ \end{array} } \right], \) we have
$$ \begin{aligned} \text{tr}\left[ {\left[ {\varvec{\theta}\times } \right]\varvec{C}} \right] & = \text{tr}\left[ {\left[ {\begin{array}{*{20}c} {c_{31} \theta_{2} - c_{21} \theta_{3} } & \times & \times \\ \times & {c_{12} \theta_{3} - c_{32} \theta_{1} } & \times \\ \times & \times & {c_{23} \theta_{1} - c_{13} \theta_{2} } \\ \end{array} } \right]} \right] \\ & = \left( {c_{23} - c_{32} } \right)\theta_{1} + \left( {c_{31} - c_{13} } \right)\theta_{2} + \left( {c_{12} - c_{21} } \right)\theta_{3} \\ & = \left[ {\begin{array}{*{20}c} {c_{23} - c_{32} } \\ {c_{31} - c_{13} } \\ {c_{12} - c_{21} } \\ \end{array} } \right]^{T}\varvec{\theta}= \left[ {\begin{array}{*{20}c} {\varvec{e}_{2}^{T} \varvec{Ce}_{3} - \varvec{e}_{3}^{T} \varvec{Ce}_{2} } \\ {\varvec{e}_{3}^{T} \varvec{Ce}_{1} - \varvec{e}_{1}^{T} \varvec{Ce}_{3} } \\ {\varvec{e}_{1}^{T} \varvec{Ce}_{2} - \varvec{e}_{2}^{T} \varvec{Ce}_{1} } \\ \end{array} } \right]^{T}\varvec{\theta}\\ \end{aligned} $$
(76)
So from (75) and (76) we obtain
$$ \text{tr}\left[ {{\text{d}}\hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} } \right] = \varvec{l}^{T}\varvec{\theta}= \varvec{l}^{T} \varvec{F}_{1}\varvec{\xi}+ \varvec{l}^{T} \varvec{F}_{2}\varvec{\zeta} $$
(77)
where
$$ \varvec{l} = \left[ {\begin{array}{*{20}c} {\varvec{e}_{2}^{T} \hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} \varvec{e}_{3} - \varvec{e}_{3}^{T} \hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} \varvec{e}_{2} } \\ {\varvec{e}_{3}^{T} \hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} \varvec{e}_{1} - \varvec{e}_{1}^{T} \hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} \varvec{e}_{3} } \\ {\varvec{e}_{1}^{T} \hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} \varvec{e}_{2} - \varvec{e}_{2}^{T} \hat{\varvec{R}}\varvec{E}_{x} \varvec{E}_{y}^{T} \varvec{e}_{1} } \\ \end{array} } \right] $$
(78)
The following can be checked algebraically,
$$ \begin{aligned} \varvec{E}_{y}^{T} \hat{\varvec{R}}{\text{d}}\varvec{E}_{x} & = \left[ {\begin{array}{*{20}c} {\Delta \varvec{y}_{1}^{T} } \\ {\Delta \varvec{y}_{2}^{T} } \\ \vdots \\ {\Delta \varvec{y}_{m}^{T} } \\ \end{array} } \right]\hat{\varvec{R}}\left[ {\begin{array}{*{20}c} {{\text{d}}\Delta \varvec{x}_{1} } & {{\text{d}}\Delta \varvec{x}_{2} } & \cdots & {{\text{d}}\Delta \varvec{x}_{m} } \\ \end{array} } \right] \\ & = \left[ {\begin{array}{*{20}c} {\Delta \varvec{y}_{1}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{1} } & {\Delta \varvec{y}_{1}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{2} } & \cdots & {\Delta \varvec{y}_{1}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{m} } \\ {\Delta \varvec{y}_{2}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{1} } & {\Delta \varvec{y}_{2}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{2} } & \cdots & {\Delta \varvec{y}_{2}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{m} } \\ \vdots & \vdots & \ddots & \vdots \\ {\Delta \varvec{y}_{m}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{1} } & {\Delta \varvec{y}_{m}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{2} } & \cdots & {\Delta \varvec{y}_{m}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{m} } \\ \end{array} } \right] \\ \end{aligned} $$
(79)
then the following holds,
$$ \begin{aligned} \text{tr}\left[ {\hat{\varvec{R}}{\text{d}}\varvec{E}_{x} \varvec{E}_{y}^{T} } \right] & = \text{tr}\left[ {\varvec{E}_{y}^{T} \hat{\varvec{R}}{\text{d}}\varvec{E}_{x} } \right] \\ & = \Delta \varvec{y}_{1}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{1} + \Delta \varvec{y}_{2}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{2} + \cdots + \Delta \varvec{y}_{m}^{T} \hat{\varvec{R}}{\text{d}}\Delta \varvec{x}_{m} \\ & = \left[ {\begin{array}{*{20}c} {\Delta \varvec{y}_{1}^{T} } & {\Delta \varvec{y}_{2}^{T} } & \cdots & {\Delta \varvec{y}_{m}^{T} } \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {\hat{\varvec{R}}} & {\mathbf{0}} & \cdots & {\mathbf{0}} \\ {\mathbf{0}} & {\hat{\varvec{R}}} & \cdots & {\mathbf{0}} \\ \vdots & \vdots & \ddots & \vdots \\ {\mathbf{0}} & {\mathbf{0}} & \cdots & {\hat{\varvec{R}}} \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {{\text{d}}\Delta \varvec{x}_{1} } \\ {{\text{d}}\Delta \varvec{x}_{2} } \\ \vdots \\ {{\text{d}}\Delta \varvec{x}_{m} } \\ \end{array} } \right] \\ & =\varvec{\eta}^{T} \left( {\hat{\varvec{R}} \otimes \varvec{I}_{m} } \right){\text{d}}\varvec{\rho}=\varvec{\eta}^{T} \left( {\hat{\varvec{R}} \otimes \varvec{I}_{m} } \right)\left( {\varvec{K} \otimes \varvec{I}_{3} } \right)\varvec{\xi}\\ \end{aligned} $$
(80)
Similarly, we have
$$ \text{tr}\left[ {\hat{\varvec{R}}\varvec{E}_{x} {\text{d}}\varvec{E}_{y}^{T} } \right] = \text{tr}\left[ {\varvec{E}_{x}^{T} \hat{\varvec{R}}^{T} {\text{d}}\varvec{E}_{y} } \right] =\varvec{\rho}^{T} \left( {\hat{\varvec{R}}^{T} \otimes \varvec{I}_{m} } \right){\text{d}}\varvec{\eta}=\varvec{\rho}^{T} \left( {\hat{\varvec{R}}^{T} \otimes \varvec{I}_{m} } \right)\left( {\varvec{K} \otimes \varvec{I}_{3} } \right)\varvec{\zeta} $$
(81)
Substituting (77), (80), and (81) into (74), we finally have (28).
