Extrinsic calibration for motion estimation using unit quaternions and particle filtering

This paper presents a method for calibration of the extrinsic parameters of a sensor system that combines a camera with an inertial measurement unit (IMU) to estimate the pendulum motion of a crane payload. The camera measures the position and orientation of a fiducial marker on the payload, while the IMU is fixed to the payload and measures angular velocity and specific force. The placements of the marker and the IMU are initially unknown, and the extrinsic calibration parameters are their position and orientation with respect to the reference frame of the payload. The calibration is done with simultaneous state and parameter estimation, where a particle filter is used for state estimation, and a Riemannian gradient descent method is used for parameter estimation. The orientation is described with unit quaternions, and gradients are developed in a Riemannian formulation based on the Lie group of unit quaternions. This leads to efficient derivations of gradient expressions involving orientations and provides added geometric insight to the problem. The efficiency of the method is demonstrated in simulations and experiments for a simplified crane payload problem.


Introduction
The motion of a rigid body can be estimated by using cameras and inertial sensors. This is important in many applications, and the topic has been thoroughly treated in the research literature. The estimation of orientation has, to a large extent, been formulated in terms of unit quaternions, and an important contribution was the multiplicative extended Kalman filter with unit quaternions (Lefferts et al., 1982), (Crassidis et al., 2007). This work was extended to particle filtering for attitude estimation in (Cheng and Crassidis, 2010).
In (Kok et al., 2017), a probabilistic approach is presented for the estimation of position and orientation using inertial sensors in combination with other sensors like cameras. They used a matrix formulation of unit quaternions for developing gradients, Jacobians and Hessians, which were applied for orientation estimation with Gauss-Newton optimization, Kalman filtering and smoothing. The matrix formulation of unit quaternions was also discussed in (Bloesch et al., 2016), where increments from a quaternion linearization point were computed with the quaternion exponential function.
In (Moakher, 2002) and (Manton, 2004) methods for calculation of the Karcher mean of rotation matrices in SO(3) were considered. Gradients were formulated in terms of the Riemannian metric on the SO(3) manifold, and the Karcher mean was obtained with a Riemannian gradient descent method, which was shown to converge to the Karcher mean under reasonable conditions. Similar results were presented in (Angulo, 2014) for nonzero quaternions. In this work, the nonzero quaternions were treated as a Lie group with a Riemannian metric, and a range of optimization problems was discussed, including the Karcher mean for nonzero quaternions.
In this work, we address the extrinsic calibration of a camera and an inertial measurement unit (IMU) using a recursive parameter estimation scheme. The method is implemented for motion estimation with a particle filter in a simplified crane payload problem, where the dynamics of the payload are described as a spherical pendulum. We formulate the extrinsic calibration as a Riemannian gradient descent problem with unit quaternions, and extend the Riemannian formulation for rotation matrices in (Moakher, 2002) and (Manton, 2004) to unit quaternions. This leads to elegant expressions that provide geometric insight into the problem. Moreover, this paper extends the results in (Myhre and Egeland, 2017) and (Myhre, 2019), where a recursive parameter estimation method based on particle filtering was used to estimate the extrinsic parameters of a fiducial marker on a swinging payload. The setup considered in this paper includes a fiducial marker and an IMU attached to the payload. The orientations of the marker and the IMU are considered unknown at the beginning of the experiment and are estimated simultaneously with the system states using the Riemannian gradient method. This paper is organized as follows. Section 2 presents preliminary results regarding particle filtering, unit quaternions, and the Riemannian gradient. The Riemannian gradient descent method is presented Section 3, and it is shown how this can be used for parameter estimation with particle filtering. Section 4 presents the dynamics and measurement models for the crane payload system and develops the gradients needed in the extrinsic calibration. Simulation results are presented in Section 5 and experimental results are presented in Section 6. Finally, the paper is concluded in Section 7.

Particle filter
Consider the discrete nonlinear dynamic system where x k ∈ R nx is the system state at time t = t k , z k ∈ R nz is the measurement and θ ∈ R n θ is the vector of static parameters. The process noise v k ∈ R nv and the measurement noise e k ∈ R ne are assumed to be uncorrelated, with probability density functions (PDF) p(v k ) and p(w k ).
The particle filter calculates an approximation of the posterior PDF p(x k |z 1:k ) using a set of N particles with state x (i) k . The starting point is the approximate posterior PDF where δ(·) is the Dirac delta function and w (i) k is the particle weighting factor. The state of a particle is propagated by drawing samples of the noise v (i) k according to its PDF, and propagating each particle state x In the measurement update, the weights are updated with the likelihood p(z k+1 |x k+1 ) as The new weights are then normalized by The posterior estimates of mean and covariance can be computed from the particle weights and states aŝ which are the first and second modes of the posterior PDF p(x k+1 |z 1: ). Finally, the resulting particles are resampled on the basis of their relative weights w (i) k+1 , which results in a new set of particles with equal weights {x This can be performed using systematic resampling as described in (Doucet and Johansen, 2009).

Parameter estimation with particle filter and gradient descent
A maximum likelihood method for parameter estimation with particle filtering was presented in (Kantas et al., 2015) and (Poyiadjis et al., 2011). The proposed solution is to use the recursive gradient search where γ ∈ R is the step size in the gradient direction. The gradient term can be calculated as in (Myhre, 2019) from It is noted that this method relies on the computation of the gradients ∇ θ log(x k |x k−1 ) and ∇ θ log p(z k |x k ).

Calculation of probability functions
In this section we will show how to find the transition PDF p(x k |x k−1 ) and the observation PDF p(z k |x k ), which are used to calculate the gradients in (9). The development will follow the method of (Kok et al., 2017). First, consider the case where the noise is additive and Gaussian, and the dynamic system is given by The transition PDF p(x k |x k−1 ) can be found as the PDF of In this case, the resulting transition PDF is Gaussian with zero mean, while there is no restriction on the state itself. In the same way, the observation PDF p(z k |x k ) is equal to the PDF of Next, consider the case where the noise is not additive, and the model is Then the transition PDF p(x k |x k−1 ) can be found as the PDF of the Gaussian noise vector v k . An expression for v k is found by solving (12) for v k . Similarly, the observation PDF p(z k |x k ) can be evaluated by solving (13) for e k , and using that p(z k |x k ) is equal to the Gaussian distribution of e k .

Quaternions
A quaternion q ∈ H can be represented as a sum of a scalar η ∈ R and a vector σ ∈ R 3 as The quaternion product between two quaternions q 1 = η 1 + σ 1 and q 2 = η 2 + σ 2 is defined as where (·) × denotes the skew symmetric matrix form of a vector The quaternion conjugate is q * = η − σ. The quaternion norm is given by The identity quaternion is defined as q id = 1 + 0 and satisfies q id ⊗ q = q ⊗ q id = q. The inverse quaternion satisfies q ⊗ q −1 = q id and is given by q −1 = q * / q 2 . A vector v ∈ R 3 can be considered as a quaternion with zero scalar part and the quaternion multiplication then gives This result is used for the computation of the quaternion kinematics, and also for the computation of elements on the tangent plane of the quaternion Lie group, presented in Section 2.5 A unit quaternion q belongs to the set H u = {q ∈ H : q = 1}. It follows that q −1 = q * if q ∈ H u . It is straightforward to verify that d dt (q * ) =η −σ = (q) * , which means that the notationq * is well defined. In addition, from d dt (q ⊗ q * ) = 0 it follows that This result is used for the derivations in Section 3.3. A unit quaternion can be written in terms of an angle φ and a unit vector k as This unit quaternions represents a rotation by an angle φ about a unit vector k. Let q IB ∈ H u be a unit quaternion describing the orientation of the body frame B with respect to the inertial frame I. Then if v B is a vector given in the coordinates of the B frame, the same vector in the coordinates of frame I will be given by The rotation matrix C(q IB ) ∈ SO(3) corresponding to the unit quaternion q IB = η + σ is given by and will satisfy v I = C(q IB )v B .

Unit quaternions as a Lie group
The set of unit quaternions H u is a Lie group where the group action is the quaternion product (Angulo, 2014). The corresponding Lie algebra g is the set of vectors u ∈ R 3 , while the elements of the tangent plane T q H u at q ∈ H u are given by q ⊗ u. The exponential map exp : g → H u is defined by the infinite series which follows from u ⊗ u ⊗ u = − u 3 . The logarithm is defined as the inverse function The exponential map can be computed from the logarithm with exp (u) = cos u + u sinc u , where sinc( u ) = sin u / u . From the Taylor series expansion of sinc( u ) = 1 − u 2 /3! + u 4 /5! . . . it can be seen that this expression is well behaved around u = 0. The logarithm can be computed from (Sola, 2017) where atan2( σ , η) is the four quadrant version of arctan( σ /η). It is straightforward to verify from a Taylor series expansion that this expression is well defined for |η| > 0, which corresponds to |φ| < π.
The kinematic differential equation of the unit quaternion q IB iṡ where ω I IB is the angular velocity of frame B with respect to frame I in the coordinates of I, and ω B IB is the same vector in the coordinates of B. We will also refer to ω I IB as the left angular velocity and to ω B IB as the right angular velocity as in (Chirikjian, 2011). It is noted thaṫ

Jacobians
The rotation matrix C = I+sin φk × +(1−cos φ)k × k × has logarithm n × = φk × . The kinematic differential equation of n is (Bullo and Murray, 1995) where Φ r (n) is the right Jacobians and Φ l (n) is the left Jacobian in SO (3). The inverse of the right Jacobian is given by (Bullo and Murray, 1995) The logarithm of the corresponding unit quaternion q = cos(φ/2) + sin(φ/2)k is u = 1 2 φk = 1 2 n. It follows from (21) that the kinematic differential equation for the logarithm of the unit quaternion isu where is the left Jacobian for unit quaternions.

Riemannian metric
The Riemannian metric can be based on an extrinsic description, where the unit quaternion manifold is embedded as the unit sphere S 3 in R 4 (Pennec, 2006). This means that the unit quaternion is described as a unit vector [η, σ T ] T in R 4 . The inner product of two quaternions can then be defined as the inner product in R 4 , which gives q 1 · q 2 = η 1 η 2 + σ 1 · σ 2 . It is noted that in this description the norm of the quaternion is q 2 = q·q. The Riemannian metric of the unit quaternions is then the restriction of the inner product of R 4 at each point of the manifold (Pennec, 2006) and is given by where (q⊗u), (q⊗v) ∈ T q H u . The Riemannian metric for the tangent space at the identity is then where u, v ∈ g. It is straightforward to show by direct calculation that this Riemannian metric is bi-invariant, which means that

Distance function
The error between two unit quaternions q 1 and q 2 can be described by the error quaternion q e = q * 1 ⊗ q 2 , where q e = cos(φ e /2) + sin(φ e /2)k e . The shortest curve on the unit sphere S 3 from q 1 to q 2 is the geodesic curve q(t) = q 1 exp(t log(q * 1 ⊗ q 2 )) for 0 ≤ t ≤ 1 (Huynh, 2009). This is known as the Slerp curve, which was introduced in (Shoemake, 1985) for the interpolation of unit quaternions. The Riemannian or geodesic distance between q 1 and q 2 is then the length of the geodesic curve, which is This distance function is well defined for |φ e | < π, which is the case if the scalar part of q * 1 ⊗q 2 is positive.

Riemannian gradient
Let f (q) be a real-valued function of a unit quaternion q. Then the gradient ∇ q f (q) is defined in terms of the Riemannian metric as the tangent vector at q which satisfies (Petersen, 1998;Moakher, 2002) q where p(t) = q ⊗ exp(tc) is a curve passing through p(0) = q with tangentṗ(0) = q ⊗ c. It is noted that the right angular velocity ω p (t) corresponding to the unit quaternion p(t) will satisfyṗ(t) = 1 2 p(t) ⊗ ω p (t), which givesṗ and ω p (0) = 2c. The gradient is in the tangent plane T q H u at q. It follows from the bi-invariance of the Riemannian metric (25), that where c and q * ⊗ ∇ q f (q) are in the Lie algebra g.

Optimization on the unit quaternion manifold
This section presents Riemannian gradient descent with unit quaternions. The presentation starts with a review of the Karcher mean problem. Then the parameter estimation method in Section 2.2 is adapted to unit quaternions. Finally, gradients with respect to orientation are presented for two quaternion objective functions, which will be used for the extrinsic calibration.

Riemannian gradient descent
A globally convergent numerical algorithm for the computation of the centre of mass on compact Lie groups was presented in (Manton, 2004). The method was formulated as a gradient descent method on a Riemannian manifold. The method was discussed in detail for rotation matrices on SO(3). In this section the corresponding solution for quaternions will be presented. This was also treated in (Angulo, 2014) for nonzero quaternions, which are quaternions that are not necessarily unit quaternions. The problem is formulated as follows. Given a set of unit quaternions q 1 , . . . , q N ∈ H u , compute the centre of massq which is the unit quaternion that minimizes A closed form solution is in general not available, and a solution can be found with the gradient descent method of (Manton, 2004), which is given for the unit quaternion case as The gradient is found to be A derivation of this is found in Section 3.3. This gives the gradient descent method which, according to (Manton, 2004), will converge to the center of mass, which is also called the Karcher mean, as long as q 1 , . . . , q N are sufficiently close to the Karcher mean to ensure that the scalar part of q * k ⊗ q i is positive for all q i .

Parameter estimation with gradient descent
The gradient search method given by (7), (8) and (9) is formulated in Euclidean space R n . In this paper we will reformulate this to include Riemannian gradient search for unit quaternions. The objective is to maximize the function f (q k ) = log p(z k |z 1:k−1 ), which can be done with the Riemannian gradient ascent algorithm The calculation (8) and (9) involves the addition and subtraction of logarithms. In the unit quaternion case this must be performed in the Lie algebra, which is done by pre-multiplying the gradients with q * . Then the gradient ∇ q log p(z k |z 1:k−1 ) can be calculated from (8) by addition and subtraction in the Lie algebra as In the same way (9) can be calculated in the Lie algebra for unit quaternions.

Quaternion gradients
In this paper, we will consider parameter estimation in combination with particle filtering. This will be done with the two objective functions f 1 (q) and f 2 (q). The first objective function is given by where v I , v B ∈ R 3 are position vectors described in different coordinate systems and W = W T ∈ R 3×3 is a symmetric weight matrix. The minimization of this objective function is used to find the unit quaternion q which describes the rotation from v B to v I . The second objective function is given by where q e = q * ⊗q is the unit quaternion which describes the difference between the true orientation q and the measured orientationq. The minimization of this objective function is used to minimize the error quaternion between q andq. This objective function differs from (30) due to the weighting matrix W, which accounts for sensor accuracy. The gradient of the first objective function f 1 (q) is found by using (16) and (28), which leads to It follows from (29) that the gradient satisfies The gradient of the second objective function is found by defining p e (t) = p(t) * ⊗q and the corresponding error logarithm u e (t) = log p e (t) , which gives f 2 p(t) = u e (t) T Wu e (t). It is noted that which means that −ω(0) = −2c is the left velocity of p e (t) at t = 0. Then from (22) This gives (41) and it follows that the gradient satisfies

Motion estimation in a crane payload system
This section considers the simplified crane payload system, as shown in Figure 1. The section presents the dynamic model of the system and the measurement models. Furthermore, the particle filter for motion estimation is given, and gradients for the extrinsic calibration of the sensors are developed.

Kinematics and dynamics
The system is modeled as a 3D spherical pendulum fixed to a moving pivot point. The payload is modeled as a point mass in the dynamic model. This means that the dynamics of rotation around the axis aligned with the cable is modeled asω z = 0, which with the addition of noise, gives a random walk. The body frame B is therefore defined as a frame with origin at the crane tip and aligned with the pendulum cable such that the pendulum mass always lies on the negative z-axis of the body frame, as shown in Figure 1. The quaternion describing the orientation of the body frame B with respect to the inertial frame I is denoted q IB , which has the kinematic differential equatioṅ The position r I m ∈ R 3 of the pendulum point mass is where r I IB is the position of the body frame with respect to the inertial frame, l is the cable length and e z = [0, 0, 1] T . The velocity iṡ A camera measures the orientation and position of the fiducial marker frame M with respect to the camera frame C. The IMU measures the angular velocity and specific force in the IMU frame A with respect to the inertial frame I. The poses of the fiducial marker and the IMU, relative to the body frame B, are unknown static parameters that are estimated simultaneously with the motion of the payload.
while the acceleration is The dynamics of the pendulum mass m is where g = 9.81m/s 2 is the gravitational constant and µ is a parameter of the cable force. Inserting (45) into (46) and solving forω B IB gives the dynamic expression for the pendulum acceleratioṅ where it is used that (e z ) × e z = 0.

Transition PDF
The motion of the system is described by (43) and (47) which are discretized to obtain where uncertainty in the system parameters and unmodeled dynamics, such as wind, friction and inertia of the payload, are accounted for by including a Gaussian noise vector v k ∼ N (0, Q). The transition PDF for this system is then given by where v k is calculated from which follows from (48) and (49).

Measurement model
Consider the case where a fiducial marker in the form of an Aruco marker (Garrido-Jurado et al., 2014) is attached to the payload. A 2D camera system is used to measure the position r C CM and the orientation q CM of the marker. Here, r C CM is the position of the marker frame M with respect to the camera frame C expressed in the coordinates of frame C, and q CM is the orientation of frame M with respect to frame C. The proposed measurement model is whereq CM andr C CM denote the measurements and e q and e r are measurement noise. From Figure 1 it can be seen that Here, q CI and r C CI describe the orientation and position of the inertial frame with respect to the camera frame and are assumed to be known. The position of the pivot point r I IB is known from the control input. The orientation q BM and position r B BM of frame M with respect to frame B are constant and assumed unknown.

Observation PDF
The observation PDF for the orientation measurement q CM,k is where e q,k = log(q * BM ⊗ q * IB,k ⊗ q * CI ⊗q CM,k ), which follows from (51) and (53). The observation PDF for the position measurementr C CM,k is where e r,k =r C CM,k − C(q CI )C(q IB,k )r B BM + C(q CI )r I IB,k + r C CI , which follows from (52) and (54). Given M independent sensor observations with uncorrelated measurement noise, the observation PDF can be calculated by combining the individual sensor observation densities as (Gustafsson, 2010) By considering the position and orientation measurements of the fiducial marker as uncorrelated, the observation PDF becomes p(z k |x k ) = p(q CM,k |x k )p(r C CM,k |x k ).

The particle filter
The orientation of the pendulum has 3 degrees of freedom, and therefore, a description with 3 parameters should be used in the particle filter state. This was done in (Cheng and Crassidis, 2010), where the state variables for orientation were the modified Rodrigues parameters of the estimation error, while the unit quaternion was used for global description. In this paper, we will instead use the logarithm δu IB,k of the quaternion estimation error to describe the estimation error with 3 parameters. This is formulated by where q IB,k is the true attitude andq IB,k is the estimate. The state vector of the particle filter is then given by where δu IB,k = log(q * IB,k ⊗ q IB,k ). In addition to the particle states x In the time propagation, the quaternions q whereω B IB,k is extracted from the state estimatex k . The propagated state vectors are then obtained by The measurement update is done with (3) and (4), where the likelihood function p(z k+1 |x k+1 ) is given by (58). After the measurement update, the state estimatex k+1 is calculated from (5). The updated pose estimate is then calculated aŝ Note that if resampling is performed, both the states x

Calibration of static parameters
The measurement models in (51) and (52) include the orientation q BM and position r B BM of the marker. These parameters may be unknown in the beginning of the experiment if the marker has been placed arbitrarily on the payload. These parameters must then be estimated to achieve motion estimation of the payload. The parameter estimation method in Section 2.2 requires computation of the gradients ∇ θ log p(x k |x k−1 ) and ∇ θ log p(z k |x k ). In this case, ∇ θ log p(x k |x k−1 ) = 0 since the transition PDF in (50) does not include the unknown parameters. The gradient of the logarithm of the observation PDF in (58) is computed in the following.
For the position r B BM the gradient is From (57) it is seen that differentiation with respect to r B BM gives Since the position r B BM and the gradient are in R 3 , the gradient method in (7) can be applied directly to simultaneously estimate the system states and the position of the marker.
For the orientation q BM the gradient is The gradient is in the tangent plane T q BM H u , and is found from Since e q,k = log(q * BM ⊗q * IB,k ⊗q * CI ⊗q CM,k ), it is seen from (37) and (42) that The gradient method in (34) can then be applied to estimate the orientation of the marker.

Inertial measurement unit
We suggest to include an IMU consisting of a 3-axis gyroscope and a 3-axis accelerometer as complementary sensors to the vision system. This can be a more robust approach than using a vision system alone, as temporary occlusions and poor lighting conditions may corrupt the visual measurements. The IMU is assumed to be rigidly attached to the payload, and the IMU frame is denoted A. The gyroscope measurementω A IB is the measured angular velocity of the payload expressed in frame A. The accelerometer measurementf A IA is the measured specific force at the location of the IMU expressed in frame A. The measurement models are given as where e ω ∼ N (0, R ω ) and e f ∼ N 0, R f are measurement noise and q AB is the unit quaternion describing the orientation of the body frame with respect to the IMU frame. Note that it is assumed that the measurements are bias free. The specific force f B IA expressed in the body frame is found as wherer B IB is the acceleration of the crane tip and is known from the control signal of the crane, r B AB is the position of the IMU with respect to frame B and g B = −gq * IB ⊗ e z ⊗ q IB is the gravity expressed in the body frame. If the crane tip is stationary, this expression reduces to The observation PDF of the IMU is found as which is the product of the densities p(ω A IB,k |x k ) ∼ N e ω,k ; 0, R ω and p(f A IA,k |x k ) ∼ N e f,k ; 0, R f . The additive noise terms e ω,k and e f,k are found from (66) and (67).
The orientation q AB of the IMU is considered to be a static parameter, which may have an unknown initial value. The gradient of the logarithm of (69) with respect to q AB is found as Since e ω,k =ω A IB,k − q AB ⊗ ω B IB,k ⊗ q * AB it can be seen from (39) that Similarly it can be seen that where e f,k =f A IA,k − q AB ⊗ f B IA,k ⊗ q AB . Multiplying (70) with q * AB and inserting (71) and (72) gives the Lie algebra element corresponding to the gradient as which can be used to estimate the orientation q AB with the gradient method in (34). The position r B AB of the IMU may also have an unknown initial value. The gradient of the logarithm of (69) with respect to r B AB gives Using that e f,k =f A IA,k − q AB ⊗ f B IA,k ⊗ q AB and inserting (68), the gradient can be found as (75)

Simulations
The performance of the proposed method for combined state and parameter estimation was validated in a simulation study, which is described in this section. First, the generation of the synthetic measurement data used in the simulation study is presented. Second, results from simulations with state estimation and nominal parameters are presented for different sensor configurations. Finally, simulations with unknown parameters and simultaneous state and parameter estimation are presented.

Particle filter
To validate the particle filter, nominal values of the static parameters were used and the parameter estimation was disabled. The particle filter parameters were set to Q = 10 −4 diag (9,9,9), N = 300 and the initial values were set to q IB,0 = q id and ω B IB,0 = [0, 0, 0] T rad s −1 . Figure 2a shows the particle filter estimates when the orientation and position measurements of the fiducial markers were considered. The likelihood was p(z k |x k ) = p(q CM,k |x k )p(r C CM,k |x k ) and the measurement noise was R q = R r = 10 −3 diag (5, 5, 5). The estimates converged to the true states after approximately t = 10 s. Figure 2b shows the particle filter estimates when only angular velocity measurements were used, so that p(z k |x k ) = p(ω A IB,k |x k ) and R ω = 10 −3 diag (1, 1, 1). In this case, the angular velocity estimates of the particle filter converged after approximately t = 15 s, while some deviation in attitude is observed. Figure 2c shows estimates when only specific force measurements were used. In this case p(z k |x k ) = p(f A IA,k |x k ) while the measurement noise was R f = 10 −4 diag (1, 1, 1). Because the specific force measurements are dominated by gravity, small errors in the estimated attitude q IB,k will cause the likelihood to become small and the particles will receive negligible weights. Thus, the estimates did not converge to the true states in this case.

Particle filter with parameter estimation
In this case, the static parameters were considered unknown, and the parameter estimation was enabled. The initial guess for the orientation and po-  The initial values of the particle filter were as described above and the likelihood of the particle filter was computed as p(z k |x k ) = p(ω A IB,k |x k )p(q CM,k |x k )p(r C CM,k |x k ), where the specific force measurements were omitted.
The gradients for the orientation q AB and position r B AB of the IMU were calculated using (73) and (75), while the gradients for the orientation BM and position r B BM of the fiducial marker were calculated using (65) and (64). The tuning parameters were set to R ω = 10 −3 diag (1, 1, 1), R f = 10 −2 diag (2, 2, 2) and R q = R r = 10 −3 diag (5, 5, 5). Figure 3a shows the state estimates of the particle filter with simultaneous parameter estimation. The estimated orientation q BM,k and position r B BM,k of the fiducial marker are plotted in Figure 3b, while the estimated orientation q AB,k and position r B AB,k of the IMU are plotted in Figure 3c. After t = 20 s the static parameters are close to their true values. This is reflected in the particle filter estimates, which also converge to the true states after approximately t = 20 s.

Experiment
The performance of the proposed method were further validated in an experimental study. In this experiment, a rectangular steel box was mounted with a wire to a static pivot point, as shown in Figure 4. The inertial frame was defined at the pivot point, such that r B IB = [0, 0, 0] T m, with the z-axis in the opposite direction of the gravity. Four Aruco markers were placed on top of the steel box such that their z-axis would always be aligned with the body frame. Measurements were only obtained from two of the markers in the experiment, where marker 1 was defined to have zero rotation about the z-axis of the body frame, which means that the q BM1 = 1 + [0, 0, 0]. Marker 2 was rotated −0.5 rad about the z-axis of marker 1 and the true orientation was q BM2 = 0.9689 + [0, 0, −0.2474]. The true positions of the markers were unknown.
A Procillica GC1020 Ethernet camera was mounted close to the pivot point and pointed downwards. The orientation of the camera was set to q CI = 0 + [0.7071, 0.7071, 0] T and the position of the camera was measured to be approximately r C CI = [0.02, −0.05, 0] T m.
The camera had a frame rate of 15 Hz and the particle filter estimates were updated with the likelihood p(z k |x k ) = p(q CM,k |x k )p(r C CM,k |x k ) every time an image was available.  The likelihood was calculated as p(z k |x k ) = p(q CM,k |x k )p(r C CM,k |x k )p(ω A IB,k |x k ).  (c) Gradient search for the static parameters of the IMU with step lengths γq = 2 × 10 −6 and γr = 2 × 10 −5 . A Bosch XDK IMU was mounted to the side of the steel box such that its reference frame was rotated π/2 rad about the body frame x-axis such that its true orientation was q AB = 0.7071 + [0, −0.7071, 0]. The true position of the IMU was not known. Angular velocity and specific force measurements were received from the IMU at a rate of 66 Hz and the particle filter estimates were updated with the likelihood p(z k |x k ) = p(ω A IB,k |x k ) at this rate. Data was recorded over a sequence of 20 s while the payload was swinging. The initial poses of the Aruco markers were set to q BM1,0 = q BM2,0 = 0.4742 + [ 0.2590, 0.7385, −0.4034] T and r B BM1,0 = r B BM2,0 = [0, 0, −1] T m, while the initial pose of the IMU was set to q AB,0 = 1 + [0, 0, 0] T and r B AB,0 = [0, 0, l] T m. The particle filter parameters and the tuning parameters Q, R q , R r and R ω were as in Section 5.3, while R f was set to R f = 10 −1 diag (2, 2, 2).
The estimated states of the particle filter are plotted in Figure 5a. The true states of the system were not known. However, it can be seen that the system converges towards a pendulum motion after approximately t = 5s. The orientation and position of marker 1 and marker 2 can be seen in Figure 5b and Figure 5c. The orientation of the markers converges to their true values after approximately t = 5 s. This is also the case for the orientation of the IMU, which is plotted in Figure 5d.

Conclusion
This paper has presented a method for calibration of the parameters that describe the position and orientation of an inertial measurement unit (IMU) and a fiducial marker mounted on a swinging payload. The states of the swinging payload were estimated using a particle filter, while a gradient descent method was used to estimate the static parameters simultaneously. Unit quaternions were used for representing orientation, and a Lie group approach was used to perform the gradient descent on the unit quaternion manifold. Furthermore, we developed gradients of the orientation of the IMU and the fiducial marker and showed how this could be used with the parameter estimation. Finally, we have presented simulations and experimental results showing that the particle filter was able to estimate the swinging pendulum motion given measurements from the considered sensors and that the parameter estimation method successfully converged to the correct parameters of the system. (a) Particle filtering with parameter estimation. The likelihood was calculated as p(z k |x k ) = p(q CM,k |x k )p(r C CM,k |x k )p(ω A IB,m |x k ).  (b) Gradient search for the static parameters of marker 1 with step lengths γq = 1 × 10 −5 and γr = 1 × 10 −5 .  (c) Gradient search for the static parameters of marker 2 with step lengths γq = 1 × 10 −5 and γr = 1 × 10 −5 .  (d) Gradient search for the static parameters of the IMU with step lengths γq = 4 × 10 −6 and γr = 1 × 10 −4 . Figure 5: Experiment with parameter estimation. The plots show the results when the static parameters were initially unknown.