A novel motion-reconstruction method for inertial sensors with constraints

Motion reconstruction for rigid bodies and rigid-body frames using data from inertial measurement units (IMUs) is a challenging task. Position and orientation determination by means of IMUs is erroneous, as deterministic and stochastic errors accumulate over time. The former of which errors can be minimized by standard calibration approaches, however, sensor calibration with respect to a common reference coordinate system to correct misalignment, has not been fully addressed yet. The latter stochastic errors are mostly reduced using sensor fusion. In this paper, we present a novel motion-reconstruction method utilizing optimization to correct measured IMU data by means of correction polynomials to minimize the deviation of motion constraints. In addition, we perform gyrometer and accelerometer calibration with an industrial manipulator to address deterministic IMU errors, especially misalignment. To evaluate the performance of the novel methods, two types of experiments, one at constant orientation and another with simultaneous translation and rotation, were conducted utilizing the manipulator. The experiments were repeated for five individual IMUs successively. Application of the calibration and optimization methods yielded an average decrease of 95% in the maximum position error compared to the results of common motion reconstruction. Moreover, the average position error over the measurement duration decreased by nearly 90%. The proposed method is applicable to velocity, position, and orientation constraints for every experiment that starts and ends at standstill.


Introduction
Reconstruction of rigid-body motion by means of measurement data acquired with a lowcost strapdown inertial measurement unit (IMU) is limited to measurements with a short time duration, as position errors due to sensor drifts and erroneous sensor axes increase is utilized to correct orientation computed by means of measured angular velocities [20,21]. On the other hand, multisensor fusion is studied, where, e.g., the computed position is corrected utilizing a global navigation satellite system (GNSS) [22,23]. As Kalman filtering requires tuning of various parameters, Madgwick proposed an approach toward simplification [24], further resulting in increased computational efficiency without accuracy loss [25].
A completely different, rarely investigated approach is based on optimization with constraints. There is a well-established approach in pedestrian tracking, known as zero-velocity update [26,27], where the fact that one foot is stationary at a time while walking is used as a constraint. Recently, the authors of the present paper proposed a similar approach, where acceleration is optimized, such that after time integration, the velocity at the end of motion equals zero [16].
This paper contributes toward motion reconstruction by means of optimization with respect to constraints and toward precise calibration. A novel method corrects measured accelerations and angular velocities by means of polynomial functions to minimize the deviation in constraints of the derived velocity, position, and orientation. In addition, a precise calibration for the accelerometer and gyrometer is performed, such that misalignment between sensors is corrected by utilizing an industrial robot for manipulation during calibration measurements. The motion reconstruction and optimization are applied to measurement data from five individual IMUs of the same type. These five IMUs were successively clamped onto the robot, calibrated, and then utilized to measure two different motions, i.e., a trajectory with motion at constant orientation and another trajectory covering simultaneous translation and rotation, with a duration of 23 seconds each. Results prior to the optimization as well as optimized data are compared to a reference trajectory provided by the robot controller to evaluate the performance of the methods.
The algorithms and methods developed in this paper can be applied to any situation where velocity, position, and orientation at the start and end are either fully or partly known. Nevertheless, the long-term objective of this work is the motion reconstruction of particles in snow avalanches. However, in snow avalanches, there is no reference data to evaluate the methods and algorithms presented in the following. Therefore, experiments utilizing an industrial manipulator may be a good start to meet this long-term objective.

Motion reconstruction
In many fields, e.g., vehicle navigation or satellite-attitude estimation, motion reconstruction is utilized for real-time prediction of trajectories. In this work, however, trajectories are computed as part of the postprocessing using IMU measurement data, which are translational acceleration, angular velocity, and magnetic-flux density. However, the magnetic field is not considered in the present work. Note that all methods are applied to calibrated accelerations and angular velocities. The latter calibrations are described in Sect. 4.
Motion reconstruction by means of IMU data can be split into a rotational part, computing rotation matrices, and a translational part, computing velocity and position, whereas the entire equations describing motion are denoted as the equations of motion (EOM).

Frame transformations
Within motion-reconstruction, data is represented in different frames, indicated by left superscripts. There is the sensor frame (S), which is attached to the IMU and thus to the measuring system. Since the measuring system is moving and the acquired data is discretized, the pose of the measuring system is given by sensor frames (S i ) for n measured time steps i, corresponding to time t i , with i ∈ {0, 1, 2, . . . , n}. Furthermore, there is the initial frame (I), which corresponds to the initial sensor frame (S 0 ). For coordinate transformation (passive rotation) from the ith sensor frame (S i ) to the initial frame (I), we introduce the transformation where u is a placeholder for translational acceleration, velocity, or position and R is a rotation matrix to perform the transformation. As I u = S 0 u, there is the special case of R 0 = I 3 , where I 3 ∈ R 3×3 denotes the identity matrix.

Computation of rotation
To obtain the orientation of the measuring system and to allow a transformation of translational data from the sensor frame to the initial frame, rotation matrices have to be computed by means of measured angular velocities S ω ω ω ∈ R 3×n and a start orientation R 0 . Thus, to determine the rotation matrix R ∈ SO (3), it is required to solve the kinematic reconstruction equation [28,29] whereṘ is the time derivative of R and Sω ω ω ∈ so(3) describes the skew symmetric matrix of angular velocities ω ω ω in the sensor fixed frame, such that ω ω ω × y =ω ω ωy for ω ω ω, y ∈ R 3 [30]. To derive a solution of Eq. (2), the well-established approach [31,32] is applied, with the terms incremental rotation vector S i , see Eq. (6), and Euler-Rodrigues formula [33] exp(˜ ) = I 3 + sinc( with the cardinal sine function [34] sinc( The Euler-Rodrigues formula, see Eq. (4), is a common approach to compute the exponential map, thus mapping elements of so(3) into SO (3). Hence, the exponential map in Eq. (3) can be interpreted as an active rotation between two successive orientations, see Fig. 1. At this point, other maps could be used instead of the exponential map, e.g., the Cayley map, as proposed in [6]. The incremental rotation vector S i is given by [35] S i applying the trapezoidal integration rule to approximate Eq. (3) in Lie algebra. The time step size t for two consecutive time steps t i and t i+1 is defined by Note that in Eq. (6), the angular velocities ω ω ω i and ω ω ω i+1 are represented in the common frame S i . However, ω ω ω i+1 is measured in frame S i+1 and therefore needs to be transformed into frame S i via Thus, the rotation vector S i from Eq. (6) is computed iteratively, as substituting Eq. (8) into Eq. (6) derives the implicit equation for k ∈ {0, 1, 2, . . . , m} iterations. As the start value for S i k=0 in Eq. (9), is applied. The number of iterations m is determined through the convergence criterion For the experiments in this paper, iterations were in the range of 3-7 depending on the magnitude of the measured angular velocities. This is consistent with the fact that higher angular velocity leads to a larger difference in two subsequent orientations at constant sampling frequency. Note that Eq. (10) generally provides a good approximation for Eq. (6) and can therefore be used to speed up the computation at the cost of minor errors in the derived rotations. Additionally, the algorithm could be initialized with S i 0 = [0 0 0] T to simplify the algorithm at the cost of one additional iteration.

Computation of velocity and position
In this section, translational velocities and positions are derived by means of time integration of measured accelerations. Due to the technology of strapdown IMUs, measured accelerations are the sum of accelerations from nongravitational forces and gravitational forces [14]. In contrast to measured accelerations of the moving measuring system, however, the gravity vector is time invariant and therefore constant with respect to the initial frame. Apparently, the gravity vector has to be eliminated to obtain accelerations, which describe the translational motion of the measuring system. The most common way to eliminate gravity is to transform measured accelerations S a ∈ R 3×n into the initial frame, where gravity is constant, and subtract gravity from accelerations where I g is the gravity 1 vector pointing toward the center of the earth. Time integration of gravity-eliminated accelerations from Eq. (12) derives defining translational velocity I v and position I p with respect to the initial frame (I), respectively. Computation of the latter time integrals utilizing the well-known explicit Euler method yields for translational velocity and position, respectively. 2 In summary, Sect. 2 derives the theoretical framework to compute velocity, position, and orientation from measured acceleration and angular velocity. An overview of the motionreconstruction method, derived in this section, is shown in Fig. 2.

Optimization of position and orientation
Although an IMU is calibrated, errors in computed positions are still increasing quadratically, foremost due to accelerometer sensor drifts. Thus, without addressing these errors, only short-time IMU measurements are significant [37]. Fortunately, optimization is possible due to constraints, which, however, differ for various applications of IMUs, e.g., pedestrian trackers [27] and snow avalanches [16]. In this paper, we investigate motion that starts and ends at standstill, resulting in physical constraints regarding translations and rotations.
The purpose of the optimization (specifically a minimization) presented in the following, is to correct measured acceleration and angular velocity by means of polynomial functions to derive velocity, position, and orientation, such that the error in constraints is minimized. Hence, we do not utilize constrained optimization [38], but an optimization algorithm to minimize errors in constraints seeking the optimal coefficients for the defined correction polynomials. The degrees of freedom (DOF), and thus the number of polynomial terms, are defined by the number of constraints, as one spatial constraint adds 3 DOF to the EOM. In the case of simultaneous translation and rotation, the latter lead to nonlinear EOM, see Eq. (14), which includes Eq. (3) and Eq. (4). To solve the nonlinear EOM we utilize the wellknown Nelder-Mead algorithm [39], which is available in Scipy [40], a common scientific library for Python. Additionally, we investigate a simplified case and solve it analytically. The simplified case results from pure translational motion at constant orientation, thus without performing any rotations, yielding linear EOM. The analytical solution is further used to validate the formulated minimization problem, see Sect. 6. It should be noted that application of the following optimization method requires an initial solution for velocity, position, and orientation derived from the motion-reconstruction method, see Sect. 2.

Constraints and correction polynomials
The measurements performed in this work always start and end at standstill, thus enabling constraints for either measured or computed rotational and translational quantities. Neglecting the Earth's motion, a standstill IMU should solely measure gravitation by means of the accelerometer and no angular velocity at all by means of the gyrometer. These constraints are partially considered with a calibration, see Sect. 4. However, constraints on acceleration and angular velocity level cannot be satisfied, as these quantities are subject to optimization, to meet constraints on translational velocity, position, and orientation level, which will be described in the following.

Rotational constraints
We already defined R 0 = I 3 in Sect. 2.1, as at time step i=0 no rotations were performed yet. At time step i=n, however, the computed orientation R n differs from a reference orientation R ref,n due to integration errors and sensor errors. Hence, denotes the error in orientation. For minimization purposes, this error in orientation is further expressed as an error in angles via the matrix logarithm [41], yielding To satisfy the constraint 3 we seek the simplest spatial polynomial with three parameters, hence a constant correction term ω c, which corrects angular velocity. In this work, the reference orientation R ref,n is provided by means of the robot controller. However, in field experiments, orientation can be determined with the help of an accelerometer and a magnetometer performing a so-called Earth-frame transformation [42,43].

Rotational correction polynomial
Correction of angular velocity, such that the computed rotation at time step n satisfies Eq. (19), is performed by means of a constant term ω c, yielding where S i ω ω ω opt and S i ω ω ω are optimized and calibrated angular velocity, respectively.

Translational constraints
For a standstill IMU after a performed measurement we denote the constraint as a nonmoving rigid body has zero velocity, yielding an error of v at the end of motion, Due to the known position by means of the robot controller (or GNSS in outdoor experiments), a further constraint is introduced, since the calculated position p n must equal the position according to the reference p ref,n provided by the robot controller (or GNSS in outdoor experiments). This leads to a position error at the end of motion according to Of course, the constraints from Eq. (21) and Eq. (23) are also valid for IMU data prior to the measurement. However, as sensor errors are zero due to a bias correction at the beginning of the measurement, the constraints are already met by proper calibration. 3 Constraint definition is indicated by the ( ! =) sign.

Translational correction polynomial
To satisfy the constraints on translational velocity and position, respectively, Eq. (21) and Eq. (23), we add a polynomial with six parameters to measured accelerations S a i , yielding optimized accelerations where a c is the correction polynomial with a coefficient 0 c for the constant term and 1 c for the linear term, respectively. Note that as we derive a solution for Eq. (27) by means of a minimization, the constraints from Eq. (21) and Eq. (23) are only satisfied to a certain extent, thus yielding a remaining error in terminal velocity and position, see Sect. 6. The same applies to the rotational constraint in Eq. (19). For mean values and standard deviations, derived for the conducted experiments, of ω c, 0 c, and 1 c, see Appendix B.

Nelder-Mead algorithm
In this work, optimizations are performed by means of the fmin function from Scipy (version 1.2.1), a package for scientific computing in Python [40]. The fmin function is an implementation of the Nelder-Mead algorithm [39] with the purpose of minimizing an objective function by variation of x. The minimization is initialized by an initial guess for x denoted as x 0 . The minimization terminates if the absolute difference of two consecutive parameters x j−1 and x j is less than or equal to a user-defined tolerance x tol and if consecutive objective function values z j−1 and z j meet the convergence criterion For the following, we define the tolerances If the minimization terminates, the evaluated parameters x that led to the smallest value of z are denoted as optimal. However, the latter parameters could be local minima if the global minimum was not found.

Correction of angular velocity
As orientation is required to compute translational velocity and position, see is performed, where θ θ θ are angles describing the error in the terminal orientation R n , derived by means of the matrix logarithm. To initialize the minimization, the start values for ω c are defined as If minimization from Eq. (33) terminates, as Eq. (30) and Eq. (31) are satisfied, optimal coefficients ω c are obtained. Therefore, the optimized angular velocity S i ω ω ω opt from Eq. (20) is utilized to compute the optimized incremental rotation vector S opt following Eq. (6). Further, the latter rotation vector is applied to Eq. (3), yielding optimized rotations Note that Eq. (33) can have multiple solutions that satisfy the constraint from Eq. (19). Consider an experiment with a duration of 2 s and a rotation about a single axis. Then, an angular velocity correction of ω c = π rad s −1 yields the same terminal orientation as ω c = 0 rad s −1 . Thus, we require that as the expected deviation of rotations is considerably smaller than one full revolution. Note that the minimization algorithm provides the option to include a second term in Eq. (33) in future work, covering orientation derived from magnetometer and accelerometer investigations [24].

Correction of translational acceleration
To compute optimal accelerations, see Eq. (28), we seek a solution of the correction coefficients 0 c and 1 c, which are derived by minimization of the objective function f ( a c), where v describes the error of velocity and p is the error of the computed terminal position compared to the reference position, see Eq. (22) and Eq. (24), respectively. In Eq. (37), w v and w p denote weights that can be adjusted to compensate for different magnitude orders of v and p, respectively. However, in this paper, the weights are equal, as the orders of magnitudes are in the same range, see Appendix A. To initialize the minimization from Eq. (37), the start values for 0 c and 1 c are defined as If the minimization from Eq. (37) terminates, optimal coefficients 0 c, 1 c are derived. Thus, the optimized translational velocity and position can be derived by computation of Eq. (13) and Eq. (14), respectively. However, optimized accelerations S a opt from Eq. (28) for optimized velocity and position, respectively. A flowchart of the optimization is shown in Fig. 3, covering the major steps.

Simplified correction without rotations
The purpose of simplification, such that the EOM rely solely on translational terms, is twofold. First, the coefficients for the polynomial correction of accelerations can be derived analytically. This further allows verification of the present optimization method from Sect. 3.4 through comparison of solutions derived by the latter method with the analytical solutions. Secondly, investigating the influence of rotations by comparing solutions for simultaneous translation and rotation with solutions for simplified motion is possible, see Sect. 6. Simplification of the general nonlinear EOM can be derived if the investigated system has constant orientation. This leads to a linear EOM, as nonlinearity is caused by rotations. Considering constant orientation, it follows that where R i is the rotation matrix that performs a transformation from sensor frame (S) to initial frame (I) and I 3 ∈ R 3×3 denotes the identity matrix. Thus, if orientation is constant over time, the sensor frame (S) corresponds to the initial frame (I). Hence, the left upper superscript is dropped in this section. By means of Eq. (42), the velocity from Eq. (40) and the position from Eq. (41) take the simplified form of respectively. Considering Eq. (22), and comparing Eq. (13) with Eq. (43) derives Therefore, Analogous to Eqs.
Rearranging Eq. (46) and Eq. (47) derives the system of equations in matrix form for computation of the polynomial coefficients,

Calibration
Calibration of low-cost IMUs is crucial, as uncalibrated IMU data can hardly be further processed to yield consistent orientation and position [12]. Therefore, each utilized IMU is subject to gyrometer and accelerometer calibration. The objective of the following calibrations is to derive parameters that relate measured quantities to ideal (reference) quantities. These parameters, in the form of matrices and vectors, can then be applied to raw IMU data from any experiment to derive calibrated data. However, parameters for accelerometer and gyrometer calibration are different and are derived from different experiments.

Error model
This section deals with error modeling of IMUs, specifically 3-axis accelerometers and gyrometers. Most of these sensor errors can be classified as scaling S, nonorthogonality N, misalignment M, and bias b, which are of a deterministic kind. A detailed description of the latter deterministic errors can be found in [44]. Additionally, there is the stochastic noise term r, which we, however, do not consider in the present calibration. Since some definitions are identical for the different sensors, left-hand indices are introduced, denoting accelerations (a) and angular velocities (ω). The error models for measured translational accelerationsā ∈ R 3×n and measured angular velocitiesω ω ω ∈ R 3×n are defined for one time step i ∈ {0, 1, 2, . . . , n} as [45] where a and ω ω ω are calibrated translational accelerations and calibrated angular velocities, respectively. As calibrated values are of interest for application and noise is not considered we define the calibration matrix then we drop the noise terms a r and ω r, and rearrange Eqs. (49)-(50), yielding ω ω ω i = ω C(ω ω ω i − ω b), ∀i ∈ {0, 1, 2, . . . , n}.
Note that in the proposed calibration, we compute the calibration matrix C. If the individual scaling S, nonorthogonality N, and misalignment M terms are of particular interest, the reader may consider [45] where the Cholesky-and LU-decomposition are used to derive the individual components.

Angle-domain gyrometer calibration
The angle-domain calibration [45,46] relies on a comparison of computed angles with reference angles. The reference angles are derived from a calibration measurement sequence containing three successive rotations about the reference axes, which correspond to the roboteffector coordinate system. In the context of calibration, we define two different coordinate systems. The coordinate system ω F is related to the wrongly scaled, nonorthogonal, and misaligned coordinate triad (x,ȳ,z). Moreover, there is the coordinate system F that is related to the reference triad (x, y, z), see Fig. 4. For elimination of the latter errors, we seek a solution of the calibration matrix ω C, which corrects computed angles regarding scaling, nonorthogonality and misalignment such that where contains predefined reference rotation vectors for three single measurements in columns. The linear independent rotation vectors of are defined as where α, β, and γ define the angle of rotation about the x-, y-, and z-axis, respectively. Hence, we rotate the IMU about the x-axis by the angle α in the first measurement, rotate about the y-axis by the angle β in the second measurement, and rotate about the z-axis by the angle γ in the third measurement. The matrix is computed from averaged, bias-corrected, measured local angular velocities from three individual measurements. Hence, each column of Eq. (57) results from the multiplication of the averaged angular velocity with the duration T = n t , similar to [9], utilizing the trapezoidal rule for averaging, in order to be consistent with the time integration, see Eq. (6). Thus,¯ represents rotations in ω F while rotating about the axes of F . Under the assumption of small errors in the angular velocities, we assume that¯ is regular. Therefore, rearranging Eq. (54) gives the calibration matrix This calibration matrix can directly be applied to the measured angular velocities, see Eq. (53). The bias ω b corresponds to the average angular velocity at standstill and is determined using j samples. Within this paper, the bias is determined at the beginning of a measurement considering 400 samples (equals 1 second). In addition, the bias is not only determined in the calibration measurements, but also in every other experiment, which can easily be achieved by persisting at standstill for at least 1 second at the beginning of a measurement. Note that nonorthogonality and misalignment errors are time invariant, however, scaling and bias errors are time and temperature dependent [45]. Thus, the best motion-reconstruction results are obtained by calibrating directly before an experiment.

Accelerometer calibration
The accelerometer coordinate system a F is also subject to wrong scaling, nonorthogonality, misalignment, and bias. Thus, we utilize k samples of measured accelerationsā from a standstill IMU at different orientations and corresponding reference accelerations a to derive a calibration matrix a C and bias a b. For acquisition of IMU accelerations and reference accelerations, see Sect. 5.4.1. To derive a solution for calibration matrix a C and bias a b we solve a least-squares minimization problem. Thus, we rearrange Eq. (52) into the steadystate form yielding Fig. 4 Reference coordinate system (x, y, z) denoted as F and wrongly scaled, nonorthogonal, and misaligned coordinate system of the sensor (x,ȳ,z) denoted as F . Angles ψ describe nonorthogonality. Angles ζ describe misalignment between an already orthogonalized sensor coordinate system (x ⊥ ,ȳ ⊥ ,z ⊥ ) and F (Color figure online) with y = a , y ∈ R 3×k (62) In Eqs. (61)-(64), y are the calibrated accelerations, P defines a parameter matrix including all calibration parameters, and x is composed of measured accelerations, and (−1) for subtraction of bias from measured accelerations. Equation (61) is an overdetermined system of equations with k equations and 12 unknown parameters. Thus, a solution for the parameter matrix P is derived by utilizing the Moore-Penrose generalized inverse [47] x + = x T (x x T ) −1 ∈ R k×4 as P ∈ R 3×4 is not a square matrix. Note that (65) is a minimal norm solution to the Least-Squares minimization problem [48] min (P) Px − y 2 2 . (66)

Measurement-data acquisition
The presented algorithms are developed with the overall objective of motion reconstruction of particles in snow avalanches, where the start-and end-orientation can be computed using magnetometer and accelerometer data [16]. Additionally, the position can be determined utilizing a GNSS. However, these orientations and positions are not valid for validation of computed trajectories as there is no reference. Thus, we use an industrial manipulator to move along predefined trajectories, where reference orientations and positions are provided by the robot controller. As compared to [8][9][10]12], calibration and experiments are performed on a manipulator without changing the clamping of the IMU in between. Hence, each IMU is calibrated with respect to the robot coordinate system, which is further used to represent the reference trajectories and evaluate the deviation of computed trajectories. Additionally, each experiment is performed five times, utilizing five individual IMUs (S 1 -S 5 ) of the same type successively. To minimize orientation and position errors due to mounting, we utilize a special flange for backlash-free clamping of the IMUs. Once an IMU is clamped, we perform two experiments for calibration followed by one experiment with motion at constant orientation and one experiment with simultaneous translation and rotation. In each experiment, we measure an additional two seconds prior and subsequent to motion, thus at standstill. This standstill data is used for sensor-bias determination and is cut out thereafter.

Measurement system setup
Measurement data from the following experiments were acquired by a measuring system called AvaNode [16].

Measuring system (AvaNode)
The AvaNode was initially built to capture the inflow dynamics of snow avalanches and provides functions that are neither used for nor affect the presented experiments, e.g., retrieval systems, GNSS, and magnetometer. However, we utilized the main component of the latter system; the IMU of type MPU9250. 4 This IMU is mounted in a 3D printed cube with 100 mm edge length, such that the IMU coordinate system coincides with the geometric center of the cube. There is also a robust housing for infield measurements, which is, however, not used in the present paper. Data provided by the IMU is stored on a SD-Card by means of an Adafruit Feather m0 microcontroller with a sampling rate of 400 Hz.

Manipulator
To perform multiple experiments with individual IMUs with the same motion, we utilized an industrial manipulator with 6 DOF of type Stäubli TX2-90L, see Fig. 5. This manipulator has a repetitive positioning accuracy of ±0.035 mm, a reach of 1200 mm, and is able to move objects ≤6 kg. In addition, the robot controller provides reference positions and orientations during motion. However, this reference data is provided with a sampling frequency of 250 Hz. Thus, we resampled measured reference data to 400 Hz.

Translational motion at constant orientation
The experiment with translational motion at constant orientation comprises three identical trajectories performed in succession with a standstill period of 1 s in between, as shown in Fig. 6. In addition, each of these three trajectories consists of three linear translations in the

Simultaneous translation and rotation
In the present section, the translational part is equivalent to the motion described in Sect. 5.2, however, with simultaneous rotations. The IMU is rotated about its axes by an angle according to Table 2. Rotation is performed such that rotational increments are evenly distributed over the course of translation. In Fig. 8, measured accelerations and angular velocities for the measurement with simultaneous translation and rotation are displayed. As in Sect. 5.2, gravity in Fig. 8a effects the y-axis of the initial frame and is eliminated following Eq. (12). In Fig. 8b, we see the correspondence of angular velocities to the rotations denoted in Table 2.

Calibration measurements
To calibrate the accelerometer and gyrometer, applying the methods from Sect. 4, it is required to perform specific measurements for calibration data acquisition.

Accelerometer measurements for six-position calibration
In this section, we present the measurements performed with the purpose of accelerometer calibration. Here, an IMU was placed in six different orientations utilizing the manipulator, such that each of the three sensor axes is successively parallel to the gravity vector and has an  Table 2 Rotations with respect to the sensor frame (S) in radians between standstill periods of the experiment with simultaneous translation and rotation. The markers denote time stamps of the standstill periods corresponding to Fig. 6 marker x-axis π / 2 0 0 π /2 0 y-axis 0 -π /2 0 -π /2 0 z-axis 0 0 -π /2 π /2 π /2 equal direction for the first three measurements and the opposite direction for the remainder. The sequence of the six orientations as we used it in this paper is shown in Fig. 9. In each orientation, the IMU was held for 5 s to measure accelerations at standstill, corresponding to reference accelerations Fig. 9 Individual orientations of the six-position calibration in chronological sequence as they were conducted in this paper. All orientations are shown with respect to a common viewpoint for orientations (1) to (6) according to Fig. 9. Note that in Eq. (67), accelerations for one time step are denoted. Further, note that due to their functionality, an axis of an IMU measures positive gravitation when this sensor axis is parallel, but points in the opposite direction to the gravitation vector. Apparently, the raw measurement data also comprises the motion from one orientation to another. Therefore, the latter motions are cut from the measurement data. The remaining data, which are measured accelerations at standstill, are arranged into a sequence resulting in an acceleration vector for calibration and corresponding reference accelerations a = 1 a 2 a 3 a 4 a 5 a 6 a .
The acquired acceleration vectorsā and a can then be utilized to compute calibration parameters, see Sect. 4.3.

Gyrometer measurements for calibration
The angle-domain calibration denotes comparison of computed angles with reference angles to derive calibration parameters. Computed angles result from time integration of the measured angular velocities and the reference angles are provided by the robot controller. For this purpose, we performed measurements containing successive rotations about the reference frame axes. In general, the rotation angles about x-, y-, and z-axes are defined as α, β, and γ , respectively. In the present gyrometer calibration measurements, we chose Therefore, Eq. (55) takes the special form of Rotations by means of the manipulator are performed such that each rotation is followed by an opposite rotation, back to the initial orientation, see Fig. 10. In addition, there is one common pivot for all rotations that coincides with the center of the reference coordinate system F . This results in a measurement comprising solely rotations without translations.
In between the individual rotations, which are performed in the sequence shown in Fig. 10, there is a standstill period of 2 s, see Fig. 15 displaying the example of IMU S 2 . Note that there is also the approach to calibrate in the angular velocity domain [12]. However, this requires that the sensor is rotated with constant angular velocity. Apparently, we could realize this approach by means of the manipulator. However, the presented approach does not require special equipment and is therefore preferred.

Experimental results
The methods for motion reconstruction and optimization, satisfying constraints on velocity, position, and angular velocity by means of polynomial corrections on measured quantities, were applied to experimental measurement data. Specifically, two different experiments (motion with and without rotation) were performed multiple times by means of an industrial robot. Data was successively acquired by five individual IMUs of the same type. Each of the IMUs underwent calibration. Additionally, reference positions and orientations were provided by the robot controller.
The presented calibration, motion reconstruction, and optimization methods were computed in Python (version 3.7) and were utilized to derive the results in the following. Inputfile and data-array handling, computation of mean values and norms was performed by means of Numpy (version 1.16.4) [49]. Reference data resampling and the Nelder-Mead optimization algorithm were utilized from Scipy (version 1.2.1) [40]. In addition, parts of Sect. 2 regarding Lie groups were computed with the help of Exudyn (version 1.0.151) [50].

Reconstructed motion without optimization
In this section, the motion-reconstruction algorithm from Sect. 2 was applied to calibrated measurement data. The latter data was acquired from two different experiments. Specifically, one experiment with translations at constant orientation and another experiment with the same translations, however, with simultaneous rotations, see Fig. 7 and Fig. 8, respectively. Each of the two experiments was performed five times, using five successively mounted IMUs of the same type, denoted as S 1 -S 5 . For every IMU, the experiment with rotations was conducted first, followed by the experiment without rotations.
The differences between the computed positions and reference positions for both experiments are shown in Fig. 11. In Fig. 11 and the following, the position errors s p err for IMU where p is a vector of spatial positions derived from Eq. (14) and p ref are corresponding reference positions provided by the robot controller. Figure 11 and the following error plots The errors in velocity are derived analogous to Eq. (72) and Eq. (73), respectively, yielding s v err andv err , see Fig. 12. Referring to the mean values of errors in Fig. 11a and Fig. 11b, we see an approx. linear and quadratic error increase, respectively. This yielded an average maximum error of 5.6 m after 23 s for the experiment at constant orientation, where the absolute maximum error was 8.7 m, see Fig. 11a. For the experiment with rotations, there was an average maximum error of 76.6 m after 23 s and a maximum error of 99 m for sensor S 4 , see Fig. 11b. Thus, for an experiment with a duration of 23 s, we investigated an approximately 13 times higher maximum error for measurements with rotations compared to measurements at constant orientation. The error values for each individual sensor are shown in Table 3 and Table 5 for experiments at constant orientation and experiments with rotations, respectively.

Optimization of experiments at constant orientation
The present section deals with optimization of reconstructed velocities and positions derived from the experiment at constant orientation, see Fig. 11a. Specifically, as R = I 3 in this case, see Eq. (42), we applied the optimization from Sect. 3.4.1 to measured calibrated accelerations. In Fig. 13a, computed positions of all IMUs, as well as a reference position are shown. Comparing Fig. 13b to Fig. 11a, it can be seen that the terminal position error was eliminated by means of the present optimization. The deviation of the position constraint, see equation Eq. (23), is in the range of 10 −9 and depends on the tolerance settings of the Nelder-Mead algorithm, see Sect. 3.2. The same applies to the deviation of the velocity constraint, see Eq. (21). For actual values of terminal velocity and position, we refer to Table 6 in Appendix A. In contrast to Sect. 6.1, the maximum error of optimized positions was approximately at measurement half-time, see Fig. 13b. Referring to the mean value in Fig. 13b, the error increased from zero to the maximum value in the first half of the measurement and decreased to zero in the second half. Additionally, the maximum of the mean error decreased from 5.6 m to 0.56 m, thus by approximately 90%, see Fig. 11a and Fig. 13b, respectively.

Validation of optimization with analytical solution
To validate the present Nelder-Mead optimization we compare the acceleration correction coefficients from Sect. 6.2 with their analytical solution, derived in Sect. 3.4.1. Table 4 shows mean values of the correction coefficients of sensor S 2 , derived from both methods.
In addition, Table 4 shows the errors of the correction coefficients, derived by means of the optimization, with respect to the analytical solution. These errors are in the range of 10 −6 and thus validate the functionality of the optimization for the present work.

Optimization of experiments with simultaneous translation and rotation
The optimization was also applied to IMU data acquired from experiments with simultaneous translation and rotation. Hence, angular velocity and acceleration were optimized to satisfy terminal orientation, velocity, and position constraints. In Fig. 14a, the positions with respect to the start position for all sensors, as well as a reference position are shown. The remaining errors subsequent to optimization are displayed in Fig. 14b. In addition, Fig. 14b provides the mean value of remaining errors from sensors S 1 -S 5 . This mean error peaked at 3.6 m at approx. measurement half-time and thus decreased by approximately 95% compared to the maximum value of the mean errors of nonoptimized positions, see Fig. 11b. However, the worst performance of the optimization was investigated for sensors S 2 and S 5 with an error decrease of approximately 92%. The optimization yielded best results for sensor S 4 , decreasing the maximum error from 99 m to 3.4 m and thus by approximately 96%. In addition, the average error over the measurement duration also decreased by approximately 93% with respect to mean values from all 5 sensors, see Table 5. As the position error is mainly driven by measurement duration, we assume that evaluation of snow-avalanche measurement data will yield errors in the range of the presented errors, i.e., between 2 m and 5 m, see Fig. 14b. Compared to typical travel distances of particles in snow avalanches, which are between 115 m and 575 m for a snow avalanche with 23 s duration [51], the relative position error is 0.35% to 4.35%.

Sensor calibration
In this section we show IMU measurement data prior and subsequent to calibration on the example of IMU S 2 .

Gyrometer calibration
The application of the angle-domain calibration, see Sect. 4.2, on raw data from the measurement described in Sect. 5.4 yields calibrated angular velocity as shown in Fig. 15b. The associated raw data is displayed in Fig. 15a. For the example of IMU S 2 , the computed calibration matrix and bias are, respectively, and were derived utilizing the angle-domain calibration from Sect. 4.2.
Note that the z-axis components, while rotating about the y-axis, see Fig. 15, result from the manipulator crossing a singularity. Comparing the values near zero from Fig. 15a and Fig. 15b, we can see an improvement regarding nonorthogonality. However, more accurate calibration may be derived considering additional rotations in the opposite direction instead of only three rotations, as in the presented calibration method. The scaling error is in the subpercent range, as shown in the diagonal of Eq. (74).

Accelerometer calibration
The raw data derived from the six-position calibration measurement, see Sect. 5.4, is shown in Fig. 16a. Applying the calibration, derived in Sect. 4.3, yields angular velocities according to Fig. 16b. The calibration parameters associated to IMU S 2 , and thus

Conclusion
Due to stochastic and deterministic errors of IMU measurement data, the deviation of the computed position and orientation increases with measurement duration. Therefore, we proposed a novel optimization method for motion reconstruction of a rigid body. This method corrects measured acceleration and angular velocity by means of correction polynomials, such that the deviations in constraints at the end of motion are minimized. To test the performance of the optimization, two experiments, each with a duration of 23 seconds, were conducted using an industrial manipulator. The two experiments, one translational motion at constant orientation and one motion with simultaneous translation and rotation, were measured by five individual calibrated IMUs of the same type successively. Immediately prior to the experiments, each IMU was calibrated by applying the six-position calibration and the angle-domain calibration to the accelerometer and gyrometer, respectively. As we utilized an industrial manipulator, misalignment between accelerometer and gyrometer is corrected with respect to a common coordinate system. The derived motion-reconstruction method and optimization method were then applied to the measurement data of the five IMUs. A comparison of results prior and subsequent to optimization provided convincing evidence in favor of the presented optimization. For the experiment at constant orientation, the optimization yielded an average maximum position error decrease of 90% for five IMU measurements. Table 6 Terminal velocity and position for the experiment at constant orientation for sensors S 1 -S 5 . v n and p n denote velocity and position prior to optimization, respectively. v opt,n and p opt,n are derived terminal velocity and position, respectively, utilizing the proposed optimization method v opt,n 2 m s −1 4.80 × 10 −9 5.59 × 10 −9 6.12 × 10 −9 6.17 × 10 −9 8.47 × 10 −9 p opt,n 2 m 8 .20 × 10 −9 7.67 × 10 −9 4.52 × 10 −9 2.84 × 10 −9 9.30 × 10 −9 Table 7 Terminal velocity and position for the experiment with simultaneous translation and rotation for sensors S 1 -S 5 . v n and p n denote velocity and position prior to optimization, respectively. v opt,n and p opt,n are derived terminal velocity and position, respectively, utilizing the proposed optimization method Moreover, optimization of simultaneous translation and rotation decreased the average maximum position error by 95%. In addition, the average position error for the experiment at constant orientation and the experiment with rotations decreased by, respectively, 86% and 93%. Thus, the results utilizing the proposed methods contribute significantly toward the minimization of trajectory deviations in inertial-motion reconstruction. Future research will be conducted regarding the application of these methods to snow-avalanche measurement data. Additionally, this method does not prevent sensor fusion; hence a combination may lead to even more accurate motion reconstruction, e.g., applying the derived optimization method to Kalman-filtered data.  Funding Open access funding provided by Austrian Science Fund (FWF). This work was conducted as part of the international cooperation project "AvaRange -Particle Tracking in Snow Avalanches" supported by the Austrian Science Fund (FWF): I 4274-N29.

Declarations
Competing Interests Johannes Gerstmayr is on the editorial board of Multibody System Dynamics and receives no compensation as a member of the editorial board.
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder.