 |
Master Thesis Code
by Simon Moser
|
Last updated: 22. April 2024
This section introduces the considerations of an optimization algorithm for initial parameter estimation. First, the measurement model is defined. Then, a nonlinear least-suares optimization algorithm is used to estimate the calibration parameters of the system.
The measurement models are based on Guang-Lin et al., (2012).
The acceleration measurement at a constant temperature can be modeled as
\begin{align*} a_x^\text{(meas)} &= b_x^{(a)} + s_{xx}^{(a)} a_{x} + s_{xy}^{(a)} a_{y} + s_{xz}^{(a)} a_{z} + s_{\omega_{xx}}^{(a)} \omega_{x} + s_{\omega_{xy}}^{(a)} \omega_{y} + s_{\omega_{xz}^{(a)}} \omega_{z} + \varepsilon_{ax} \\[3mm] a_y^\text{(meas)} &= b_y^{(a)} + s_{yx}^{(a)} a_{x} + s_{yy}^{(a)} a_{y} + s_{yz}^{(a)} a_{z} + s_{\omega_{yx}}^{(a)} \omega_{x} + s_{\omega_{yy}}^{(a)} \omega_{y} + s_{\omega_{yz}^{(a)}} \omega_{z} + \varepsilon_{ay} \\[3mm] a_z^\text{(meas)} &= b_z^{(a)} + s_{zx}^{(a)} a_{x} + s_{zy}^{(a)} a_{y} + s_{zz}^{(a)} a_{z} + s_{\omega_{zx}}^{(a)} \omega_{x} + s_{\omega_{zy}}^{(a)} \omega_{y} + s_{\omega_{zz}^{(a)}} \omega_{z} + \varepsilon_{az}, \end{align*}
where
with \(i,j \in \{x,y,z\}\).
The gyroscope measurement at a constant temperature can be modeled as
\begin{align*} \omega_x^\text{(meas)} &= b_x^{(\omega)} + s_{xx}^{(\omega)} \omega_{x} + s_{xy}^{(\omega)} \omega_{y} + s_{xz}^{(\omega)} \omega_{z} + s_{a_{xx}}^{(\omega)} a_{x} + s_{a_{xy}}^{(\omega)} a_{y} + s_{a_{xz}}^{(\omega)} a_{z} + s_{a_{2x}}^{(\omega)} a_x^2 + \varepsilon_{\omega x} \\[3mm] \omega_y^\text{(meas)} &= b_y^{(\omega)} + s_{yx}^{(\omega)} \omega_{x} + s_{yy}^{(\omega)} \omega_{y} + s_{yz}^{(\omega)} \omega_{z} + s_{a_{yx}}^{(\omega)} a_{x} + s_{a_{yy}}^{(\omega)} a_{y} + s_{a_{yz}}^{(\omega)} a_{z} + s_{a_{2y}}^{(\omega)} a_y^2 + \varepsilon_{\omega y} \\[3mm] \omega_z^\text{(meas)} &= b_z^{(\omega)} + s_{zx}^{(\omega)} \omega_{x} + s_{zy}^{(\omega)} \omega_{y} + s_{zz}^{(\omega)} \omega_{z} + s_{a_{zx}}^{(\omega)} a_{x} + s_{a_{zy}}^{(\omega)} a_{y} + s_{a_{zz}}^{(\omega)} a_{z} + s_{a_{2z}}^{(\omega)} a_z^2 + \varepsilon_{\omega z}, \end{align*}
where
with \(i,j \in \{x,y,z\}\).
If we only consider the accelerometer and gyroscope measurements during zero velocity periods, we know that
\begin{align*} \omega_i &= 0, \quad \forall \ i \in \{x,y,z\} \\ || a ||_2 &= g, \quad \text{where } g \text{ is the gravitational acceleration}. \end{align*}
Thus, the measurement models can be simplified to
\begin{align*} a_x^\text{(meas)} &= b_x^{(a)} + s_{xx}^{(a)} a_{x} + s_{xy}^{(a)} a_{y} + s_{xz}^{(a)} a_{z} + \varepsilon_{ax} \\[3mm] a_y^\text{(meas)} &= b_y^{(a)} + s_{yx}^{(a)} a_{x} + s_{yy}^{(a)} a_{y} + s_{yz}^{(a)} a_{z} + \varepsilon_{ay} \\[3mm] a_z^\text{(meas)} &= b_z^{(a)} + s_{zx}^{(a)} a_{x} + s_{zy}^{(a)} a_{y} + s_{zz}^{(a)} a_{z} + \varepsilon_{az}, \end{align*}
and
\begin{align*} \omega_x^\text{(meas)} &= b_x^{(\omega)} + s_{a_{xx}}^{(\omega)} a_{x} + s_{a_{xy}}^{(\omega)} a_{y} + s_{a_{xz}}^{(\omega)} a_{z} + s_{a_{2x}}^{(\omega)} a_x^2 + \varepsilon_{\omega x} \\[3mm] \omega_y^\text{(meas)} &= b_y^{(\omega)} + s_{a_{yx}}^{(\omega)} a_{x} + s_{a_{yy}}^{(\omega)} a_{y} + s_{a_{yz}}^{(\omega)} a_{z} + s_{a_{2y}}^{(\omega)} a_y^2 + \varepsilon_{\omega y} \\[3mm] \omega_z^\text{(meas)} &= b_z^{(\omega)} + s_{a_{zx}}^{(\omega)} a_{x} + s_{a_{zy}}^{(\omega)} a_{y} + s_{a_{zz}}^{(\omega)} a_{z} + s_{a_{2z}}^{(\omega)} a_z^2 + \varepsilon_{\omega z}. \end{align*}
Even tough these equations are simplified, there are still twelve or fifeteen unknown parameters, respevtively. Therefore, a calibration sequence of at least fifeen different known poses is required to estimate all parameters.
A duable calibration sequence is to put the sensor on a flat surface where every face is pointing upwards once. If we consider such a calibration sequence where the sensor is in six different poses, where always one axis is aligned with the gravitational acceleration, the calibration model must be further simplified.
Since for our sensors (MPU-9250 and ADXL375) the cross-axis sensitivities are assumed to be smaller than the scale factors, we neglect the cross-axis sensitivities. Furthermore, we assume that the gyroscope is not very sensitive to the accelerometer measurements (small cross-sensor sensitivities). Thus, the measurement models can be further simplified to
\begin{align*} a_x^\text{(meas)} &= b_x^{(a)} + s_{xx}^{(a)} a_{x} + \varepsilon_{ax} \\[3mm] a_y^\text{(meas)} &= b_y^{(a)} + s_{yy}^{(a)} a_{y} + \varepsilon_{ay} \\[3mm] a_z^\text{(meas)} &= b_z^{(a)} + s_{zz}^{(a)} a_{z} + \varepsilon_{az} \end{align*}
for the accelerometer and
\begin{align*} \omega_x^\text{(meas)} &= b_x^{(\omega)} + \varepsilon_{\omega x} \\[3mm] \omega_y^\text{(meas)} &= b_y^{(\omega)} + \varepsilon_{\omega y} \\[3mm] \omega_z^\text{(meas)} &= b_z^{(\omega)} + \varepsilon_{\omega z} \end{align*}
for the gyroscope.
A function jumpEstimateCalibrationParameters.m is implemented to estimate the calibration parameters. This function takes sensor data and a zero velocity detection vector as inputs. The general functionality is as follows:
minZvDuration).maxZvDuration).The function jumpEstimateCalibrationParameters.m is based on the MATLAB function lsqlin to solve the optimization problem.
An example of the usage of the function is shown in derivation7IntialParamEst.m. There, the parameters are estimated three times:
The output of the text in the example is shown below.