Localization of Stereovision for Measuring In-Crash Toeboard Deformation

Abstract

This paper presents a technique to localize a stereo camera for in-crash toeboard deformation measurement. The proposed technique designed a sensor suite to install not only the stereo camera but also initial measurement units (IMUs) and a camera for localizing purpose. The pose of the stereo camera is recursively estimated using the measurement of IMUs and the localization camera through an extended Kalman filter. The performance of the proposed approach was first investigated in a stepwise manner and then tested in controlled environments including an actual vehicle crash test, which had successfully resulted in measuring the toeboard deformation during a crash. With the oscillation motion in the occurrence of the crash captured, the deformation of the toeboard measured by stereo cameras can be described in a fixed coordinate system.

1. Introduction

The incidence of vehicle crashes keeps increasing with the production of vehicles, and more than six million crashes are reported every year these days in the United States, about thirty percent of which come with fatality and injury [1]. Vehicle crash is a severe and complicated dynamic phenomenon due to the complexity of physical interaction between the structures resulting from the impact. It is indispensable that the vehicles be crash-tested in indoor facilities and that the test results be used to design vehicles with improved crashworthiness [2,3,4,5,6]. In the crash tests, one of the focuses is laid on the toeboard deformation [7,8], which is due to the strong correlation between toeboard intrusion and lower extremity injuries, as seen both from collision statistics [9,10] and crash simulation [11]. A detailed analysis of toeboard deformation during a crash test can disclose useful intricacies about the otherwise unknown effect of the crash phenomenon.

In general, past work on vehicle deformation measurement can be classified into two approaches. In the first approach, finite element analysis (FEA) and other computational mechanics analyses have been used to predict instead of measuring the deformation. The advantage of numerical analysis is its ability to simulate all types of crash tests ([12,13,14,15]). Hickey and Xiao [16] used the three-dimensional (3D) model of a commercial vehicle and performed FEA to examine the effects of the deformation during a car crash test. Employing the full-scale 3D model directly for crash analysis and subsequent computational design requires large computational loads. Yang et al. [17] utilized the response surface method for a complex and dynamic large deformation problem to accelerate the analysis and design process. Pre- and post-measurements from the actual vehicle crash tests were used by Cheng et al. [18] and McClenathan et al. [19] to provide boundary conditions for FEA. Zhang et al. [20] reported the reconstruction of the deformation of a vehicle in a crash accident using high-performance parallel computing to incorporate an advanced elastic–plastic model. While the FEA has grown to be ever more important, its ability to estimate detailed deformations such as toeboard deformation is significantly limited because of the presence of various computation errors and the lack of actual measurements.

The second approach is based on the direct measurement. In the approach, vision-based measurement became most popular because of the ability of the camera to capture information of a continuous field ([21,22,23,24,25]). Digital image correlation, originally proposed by Sutton et al. [26], projects an electronic speckle pattern to the specimen to derive the deformation of the specimen from the displacement of the image of the speckle pattern on the specimen, and it has been widely employed to measure small deformation such as structure strain [27], cracks identification [28,29], or vibration of structures [30,31], in a static environment. Schmidt et al. [32] employed a stereo vision-based technique to measure the time-varying deformation of specimens in gas gun impact tests. As an alternative to the digital image correlation (DIC), Iliopoulos and Mchopoulos [33] developed the mesh-free random grid technique to measure deformation using marked dots, which was later referred to as Dot Centroid Tracking (DCT) for comparison with the DIC [34]. These techniques observed the deformation of a surface fixed to a static base from a static viewpoint. In general, while deformation measurement was well studied, the techniques cannot be applied to toeboard deformation measurement directly because of the oscillatory motion of the cameras. Since the line of sight of the toeboard is significantly limited, the cameras must be fixed near the toeboard to some part of the vehicle body, which deforms and oscillates by the crash. The measurement of toeboard through a stereo camera is therefore subject to the relative motion of the camera itself.

This paper presents a technique to localize a stereo camera for in-crash toeboard deformation measurement and a design to implement the technique in vehicle crash tests using a recursive state estimation [35,36,37]. This state estimation technique further completes our previous work that localizes the sensor suite using only the camera pose obtained through a computer vision technique [7], as the excessive oscillation of the localization camera due to the non-rigidity of the sensor suite poses extra errors in the measured toeboard deformation. In addition to the cameras for deformation measurement and localization, the proposed technique uses additional inertial measurement units (IMUs) for the localization. Having all the sensors in a rigid structure, the proposed technique implements extended Kalman filter (EKF) and estimates the pose of the stereo camera in the global coordinate frame using the observations of the downward camera and IMUs. In-crash toeboard deformation can thus be measured by subtracting the estimated stereo camera pose from the observed toeboard deformation. A sensor suite comprising the stereo camera, downward camera, and the IMUs is designed and placed on the seat mount such that the rigid body assumption is valid. The downward camera is located underneath the vehicle floor by making a hole.

This paper is organized as follows. The next section defines the state estimation problem of concern. Section 3 presents the proposed EKF-based technique to localize the stereo camera for the in-crash toeboard deformation measurement. Section 4 introduces the hardware design for our approach. The experimental results are then presented in Section 5. Conclusions and ongoing work are summarized in Section 6.

2. Sensor Suite and Localization Problem of Stereovision

2.1. Problem Formulation

Figure 1 illustrates the fundamental design of the sensor suite adopted in this paper and the major components that address the problem of localizing the stereo camera. The stereo camera is fixed to the camera fixture, which is assumed to be rigid, and located such that it sees the toeboard. Additionally fixed to the camera fixture are the downward camera and the IMUs adjacent to the cameras, and a checkerboard is taped to the floor to localize the downward camera. The IMUs each consist of a triaxial gyroscope and an accelerometer. While the stereo camera measures the toeboard deformation in its camera coordinate frame, the downward camera and the IMUs are the sensors available to localize the pose of the stereo camera. The stereovision localization problem is thus defined as identifying the stereo camera pose given the images of the downward camera and the readings of the IMUs.

2.2. Linear Acceleration of Sensor Suite

In accordance with the rigid body dynamics, the acceleration ($\mathit{a}$) at the positions of IMUs is related to that of the center of the sensor suite by

$$\mathit{a}={\mathit{a}}_c+\dot{\mathit{\omega }}\times \mathit{r}+\mathit{\omega }\times (\mathit{\omega }\times \mathit{r}),$$

(1)

where ${\mathit{a}}_c$ is the acceleration of the center of the sensor suite body, $\mathit{\omega }$ is the angular velocity of the sensor suite body frame, and $\mathit{r}$ is the relative position of the IMU to the center.

Note that it is more convenient to describe the acceleration relation (1) in the body frame, and that the cross-product identity $U({\mathit{v}}_1\times {\mathit{v}}_2)=\left(U{\mathit{v}}_1\right)\times \left(U{\mathit{v}}_2\right)$ holds for unitary matrix $U\in {\mathbb{R}}^{3\times 3}$. By left multiplying the transformation matrix from the inertial frame to the body frame, the acceleration relation (1) can be rewritten as

$${\mathit{a}}^B={\mathit{a}}_c^B+{\dot{\mathit{\omega }}}^B\times {\mathit{r}}^B+{\mathit{\omega }}^B\times ({\mathit{\omega }}^B\times {\mathit{r}}^B),$$

(2)

where ${\mathit{a}}^B$, ${\dot{\mathit{\omega }}}^B$, ${\mathit{\omega }}^B$ and ${\mathit{r}}^B$ are the linear acceleration, angular acceleration, angular velocity, and position in the body frame (${}^B$).

The linear accelerations at different position of a rigid body need the input of angular acceleration. The input can be derived from the measurements of the triaxial accelerometers and gyroscopes in the IMUs. Because the derivation of angular acceleration at a point requires two IMU measurements, consider two IMUs in addition to the reference point as indicated in Figure 2. From Equation (1), the difference of the angular acceleration of IMU 1 from that of IMU 2 is given by

$${\mathit{a}}_2-{\mathit{a}}_1=\dot{\mathit{\omega }}\times ({\mathit{r}}_2-{\mathit{r}}_1)+\frac{1}{2}{\mathit{\omega }}_2\times \left({\mathit{\omega }}_2\times {\mathit{r}}_2\right)-\frac{1}{2}{\mathit{\omega }}_1\times \left({\mathit{\omega }}_1\times {\mathit{r}}_1\right),$$

(3)

where ${\mathit{a}}_1,{\mathit{a}}_2,{\mathit{\omega }}_1$, and ${\mathit{\omega }}_2$ are the accelerations and angular velocities at IMU 1 and 2, respectively. ${\mathit{r}}_1$ and ${\mathit{r}}_2$ are each the vector from the reference point to them. The position of the reference point for each pair of IMUs is chosen to be the midpoint of the IMU positions: $-{\mathit{r}}_1={\mathit{r}}_2=\frac{1}{2}{\mathit{r}}_{12}\equiv \frac{1}{2}\mathit{r}$. This is to minimize the complexity and the error of transformation. The substitution of the midpoint into Equation (3) yields

$$C\left(\mathit{r}\right)\dot{\mathit{\omega }}=\mathit{b}\equiv {\mathit{a}}_1-{\mathit{a}}_2+\frac{1}{2}{\mathit{\omega }}_2\times ({\mathit{\omega }}_2\times \mathit{r})+\frac{1}{2}{\mathit{\omega }}_1\times ({\mathit{\omega }}_1\times \mathit{r}),$$

(4)

where the skew-symmetric cross-product matrix $C\left(\mathit{r}\right):{\mathbb{R}}^3\to {\mathbb{R}}^{3\times 3}$ is defined as

$$C\left(\mathit{r}\right)=\left(\begin{array}{ccc}0& -r_3& r_2\\ r_3& 0& -r_1\\ -r_2& r_1& 0\end{array}\right),$$

(5)

where $\mathit{r}={(r_1,r_2,r_3)}^T$. The angular acceleration $\dot{\mathit{\omega }}$ can be computed through the IMU measurements of ${\mathit{a}}_1$, ${\mathit{a}}_2$, ${\mathit{\omega }}_1$, and ${\mathit{\omega }}_2$ by solving the linear problem (4).

2.3. Localization of Downward Camera

The installation of the downward camera and the checkerboard on the ground is because the ground is the closest static surrounding structure to the sensor suite and thus can be measured most accurately. As illustrated in Figure 3, the global frame can be set at one end of the checkerboard and used to localize the downward camera. The downward camera observes the checkerboard pattern and any other extractable features. By associating the pattern and features between the global frame and pixel frame, the pose of the downward camera with respect to the global frame can be identified after calibrating the camera parameters following the procedures proposed by Zhang [38] or Heikkilä and Silvén [39]. In particular, for the checkerboard attached to the ground, the checkerboard corner points are readily identified [40] and serve as the planar pattern for the camera pose measurement, as illustrated in Figure 3.

For the global localization of the downward camera, let the pixel coordinates of the corner point $(i,j)$ at the kth time step be ${\mathit{p}}_{k,ij}$. The pixel coordinates can be related to the corner point in the global frame by

$${\mathit{p}}_{k,ij}\to \left[\left(\begin{array}{c}{\mathit{N}}_k\\ 0\end{array}\right)+\left(\begin{array}{c}i\\ j\\ 0\end{array}\right)\right]w,$$

(6)

where w is the width of tiles of the checkerboard. ${\mathit{N}}_k$ is the number of tiles of the origin of the detected checkerboard at the current step relative to the origin at the first step, which can be accumulated by

$${\mathit{N}}_k={\mathit{N}}_{k-1}+\mathrm{Integer}\left(\frac{{\Delta }_k+{\mathit{p}}_{k,0}-{\mathit{p}}_{k-1,0}}{{\overline{\Delta }}_{tile}}\right),$$

(7)

where ${\Delta }_k$ is the motion between two images in the pixel coordinates, and ${\mathit{p}}_{k,0}$ is the origin of the detected checkerboard origin in the pixel coordinate in image k. The initial ${\mathit{N}}_0$ is ${(0,0)}^T$.

Figure 3. Scheme for downward camera localization.

Figure 3. Scheme for downward camera localization.

The design and sensor settings adopted in this paper allow the motion of the sensor suite to be predicted with respect to the body frame and the pose of the downward camera to be measured in the global frame. While the downward camera is fixed to the sensor suite and thus can measure the sensor suite pose through the kinematic transformation, the pose predicted by the motion model and the angular acceleration measurement would be different from the pose measured by the downward camera and the kinematic transformation. The primary reason is the dead-reckoning errors stemming from the motion prediction with angular acceleration measurements. In addition, the pose of the downward camera alone contains excessive oscillations because of the non-rigidity of the sensor frame in the crash test. The acceleration measurement is in general noisy, and its integration creates notable integration errors, which accumulate over time in the name of dead-reckoning errors. Since the pose of the downward camera can be accurately measured because of its close distance to the ground, it is essential to estimate the pose of the sensor suite or the stereo camera by integrating the motion prediction and the observation. The next section presents the localization of a stereo camera proposed and formulated in the framework of EKF.

3. EKF-Based Localization of Stereo Camera for Toeboard Measurement

3.1. Overview

Figure 4 shows the EKF-based localization of the stereo camera proposed in this paper for toeboard deformation measurement. Since the camera fixture is assumed to be rigid and can transform the body frame to the stereo camera, to be identified are the position and orientation of the body frame, $\{\mathit{p},\mathit{\theta }\}$, where $\mathit{p}\in {\mathbb{R}}^3$ is the position of the origin of the body frame and $\mathit{\theta }\in {\mathbb{R}}^3$ is its Euler angle. To recursively estimate the motion of sensor suite using EKF framework, the state $\mathit{x}\equiv \{\mathit{p},\dot{\mathit{p}},\ddot{\mathit{p}},\mathit{\theta },{\mathit{\omega }}^B,{\dot{\mathit{\omega }}}^B\}$ includes not only the pose, but also velocity $\dot{\mathit{p}}$, acceleration $\ddot{\mathit{p}}$, angular velocity ${\mathit{\omega }}^B$, and angular acceleration ${\dot{\mathit{\omega }}}^B$ in the body frame.

In accordance with the EKF, the primary processes of estimation are the prediction and the correction. The prediction of the state at discrete time k, or the derivation of the mean ${\mathit{x}}_{k|k-1}$ and the covariance $P_{k|k-1}$, is performed using the motion model of a sensor suite and its state estimated at $k-1$, i.e., ${\mathit{x}}_{k-1|k-1}$ and $P_{k-1|k-1}$. In the correction step, the predicted sensor suite state is corrected to ${\mathit{x}}_{k|k}$ and $P_{k|k}$ by fusing the measurements of IMUs (${\mathit{a}}_k$ and ${\mathit{\omega }}_k$), downward camera pose (${\mathit{p}}_k^C$ and ${\mathit{\theta }}_k^C$), and angular acceleration (${\dot{\mathit{\omega }}}_k$) described in Section 2.2 and Section 2.3. While different sensors are used for the same state, Kalman gain is adjusted through the statistic property of motion and sensor noise via their covariance $P_{k|k-1}$ and ${\Sigma }_{k,\mathit{v}}$ and provides the optimal estimate for each state in the proposed approach. Once the pose of the sensor suite is globally estimated, the deformation of the toeboard can be measured with respect to the global and any other coordinate frames.

The operation of the EKF needs motion and sensor models. The discrete motion model of the sensor suite and the observation model subjected to uncertainty are generically given by

$${\mathit{x}}_k=\mathit{f}({\mathit{x}}_{k-1},{\mathit{w}}_k),\mathrm{and}{\mathit{z}}_k=\mathit{h}({\mathit{x}}_k,{\mathit{v}}_k),$$

(8)

where $\mathit{f}(·)$ and $\mathit{h}(·)$ are the respective motion and sensor models; ${\mathit{w}}_k$ and ${\mathit{v}}_k$ represent the motion model noise and sensor model noise, respectively. The subsequent two sections present the motion and sensor models developed in the proposed approach.

3.2. Motion Model of Sensor Suite

The motion of the sensor suite pose is determined by the motion and the deformation of the entire vehicle, which cannot be modeled and identified easily. Meanwhile, the sensor suite motion is constrained by the motion and the deformation of the vehicle. This means that the range of the sensor suite motion is bounded. With a short time step $\Delta t$, the proposed approach accordingly predicts the pose of the sensor suite by the random walk motion model as

$$\begin{array}{cc}\hfill {\mathit{p}}_k& ={\mathit{p}}_{k-1}+\Delta t{\dot{\mathit{p}}}_{k-1}+\frac{1}{2}\Delta t^2\left({\ddot{\mathit{p}}}_{k-1}+{\mathit{ϵ}}_{k,\ddot{\mathit{p}}}\right),\hfill \\ \hfill {\dot{\mathit{p}}}_k& ={\dot{\mathit{p}}}_{k-1}+\Delta t\left({\dot{\mathit{p}}}_{k-1}+{\mathit{ϵ}}_{k,\ddot{\mathit{p}}}\right),\hfill \\ \hfill {\ddot{\mathit{p}}}_k& ={\ddot{\mathit{p}}}_{k-1}+{\mathit{ϵ}}_{k,\ddot{\mathit{p}}},\hfill \\ \hfill {\mathit{\theta }}_k& ={\mathit{\theta }}_{k-1}+\Delta tE\left({\mathit{\theta }}_k\right)\left({\mathit{\omega }}_{k-1}^B+\Delta t({\dot{\mathit{\omega }}}_{k-1}^B+{\mathit{ϵ}}_{k,\dot{\mathit{\omega }}})\right),\hfill \\ \hfill {\mathit{\omega }}_k^B& ={\mathit{\omega }}_{k-1}^B+\Delta t\left({\dot{\mathit{\omega }}}_{k-1}^B+{\mathit{ϵ}}_{k,\dot{\mathit{\omega }}}\right),\hfill \\ \hfill {\dot{\mathit{\omega }}}_k^B& ={\dot{\mathit{\omega }}}_{k-1}^B+{\mathit{ϵ}}_{k,\dot{\mathit{\omega }}},\hfill \end{array}$$

(9)

where $\left\{{\mathit{ϵ}}_{k,\ddot{\mathit{p}}},{\mathit{ϵ}}_{k,\dot{\mathit{\omega }}}\right\}$ is a motion noise due to unresolved linear and angular acceleration, and $\mathit{\theta }={\left[\varphi ,\theta ,\phi \right]}^{\top }$ are the Euler angles corresponding to the roll, pitch, and yaw motion of the sensor suite. In the equation

$$\mathit{E}\left(\mathit{\theta }\right)=\left(\begin{array}{ccc}1& sin\left(\varphi \right)tan\left(\theta \right)& cos\left(\varphi \right)tan\left(\theta \right)\\ 0& cos\left(\varphi \right)& -sin\left(\varphi \right)\\ 0& sin\left(\varphi \right)/cos\left(\theta \right)& cos\left(\varphi \right)/cos\left(\theta \right)\end{array}\right),$$

(10)

is the Euler angle rates matrix [41], whose multiplication with the body-fixed angular velocity yields Euler angle rates. Through linearization, the motion model can be encapsulated into a canonical form as:

$${\mathit{x}}_k=F_k{\mathit{x}}_{k-1}+V_k{\mathit{w}}_k,$$

(11)

where $F_k$ and $V_k$ are the Jacobian matrix of the motion and the motion noise term, respectively, and are given by

$$F_k=\left(\begin{array}{cc}{F}_1& {\mathbf{0}}_{9\times 9}\\ {\mathbf{0}}_{9\times 9}& F_{2,k}\end{array}\right),\mathrm{and}{V}_k=\left(\begin{array}{cc}{V}_1& {\mathbf{0}}_{9\times 3}\\ {\mathbf{0}}_{9\times 3}& V_{2,k}\end{array}\right),$$

(12)

where

$$\begin{array}{ccc}& F_1=\left(\begin{array}{ccc}{\mathit{I}}_3& \Delta t{\mathit{I}}_3& \frac{\Delta t^2}{2}{\mathit{I}}_3\\ {\mathbf{0}}_3& {\mathit{I}}_3& \Delta t{\mathit{I}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathit{I}}_3\end{array}\right),& F_{2,k}=\left(\begin{array}{ccc}{\mathit{I}}_3& \Delta t\mathit{E}\left({\mathit{\theta }}_k\right)& \Delta t^2\mathit{E}\left({\mathit{\theta }}_k\right)\\ {\mathit{0}}_3& {\mathit{I}}_3& \Delta t{\mathit{I}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathit{I}}_3\end{array}\right),\hfill \\ & V_1=\left(\begin{array}{c}\frac{\Delta t^2}{2}{\mathit{I}}_3\\ \Delta t{\mathit{I}}_3\\ {\mathit{I}}_3\end{array}\right),& V_{2,k}=\left(\begin{array}{c}\Delta t^{2}E\left({\mathit{\theta }}_k\right)\\ \Delta t{\mathit{I}}_3\\ {\mathit{I}}_3\end{array}\right).\hfill \end{array}$$

${\mathit{w}}_k\sim \mathcal{N}\left(\mathit{0},{\Sigma }_{k,\mathit{w}}\right)$ is a motion noise, which is validly assumed to be Gaussian since $\Delta t$ is small.

3.3. Sensor Models for Localization of Sensor Suite

3.3.1. Sensor Models of Accelerometer

According to Equation (2), the sensor model outputting linear acceleration with respect to the body frame can be approximated with Gaussian noise as

$$\begin{array}{c}\hfill {\mathit{z}}_{k,a}={\mathit{h}}_{k,a}^{\ddot{\mathit{p}}}({\mathit{x}}_k,{\mathit{r}}^B)+{\mathit{v}}_a\equiv R{\left({\mathit{\theta }}_k\right)}^T{\ddot{\mathit{p}}}_k+{\dot{\mathit{\omega }}}_k^B\times {\mathit{r}}^B+{\mathit{\omega }}_k^B\times ({\mathit{\omega }}_k^B\times {\mathit{r}}^B)+{\mathit{v}}_a,\end{array}$$

(13)

where ${\mathit{z}}_{k,a}$ is the measurement of linear acceleration, ${\mathit{r}}^B$ is the position of accelerometer in the body frame, and ${\mathit{v}}_a\sim \mathcal{N}(\mathbf{0},{\Sigma }_a)$ is the measurement noise of the accelerometer. $R\left(\mathit{\theta }\right)$ is the rotation matrix [41] to transfer the body frame to the inertial frame.

Because of the association through integration, the measurement ${\mathit{z}}_{k,a}$ can also be represented with the velocity and the position. This additionally introduces two sensor models:

$$\begin{array}{cc}\hfill {\mathit{z}}_{k,a}& ={\mathit{h}}_{k,a}^{\mathit{p}}({\mathit{x}}_k,{\mathit{r}}^B)+{\mathit{v}}_a\equiv R{\left({\mathit{\theta }}_k\right)}^T\frac{{\mathit{p}}_k-2{\mathit{p}}_{k-1}+{\mathit{p}}_{k-2}}{\Delta t^2}+{\dot{\mathit{\omega }}}_k^B\times {\mathit{r}}^B+{\mathit{\omega }}_k^B\times ({\mathit{\omega }}_k^B\times {\mathit{r}}^B)+{\mathit{v}}_a,\hfill \\ \hfill {\mathit{z}}_{k,a}& ={\mathit{h}}_{k,a}^{\dot{\mathit{p}}}({\mathit{x}}_k,{\mathit{r}}^B)+{\mathit{v}}_a\equiv R{\left({\mathit{\theta }}_k\right)}^T\frac{{\dot{\mathit{p}}}_k-{\dot{\mathit{p}}}_{k-1}}{\Delta t}+{\dot{\mathit{\omega }}}_k^B\times {\mathit{r}}^B+{\mathit{\omega }}_k^B\times ({\mathit{\omega }}_k^B\times {\mathit{r}}^B)+{\mathit{v}}_a.\hfill \end{array}$$

(14)

Let the measurements and the sensor models be described collectively as

$${\mathit{z}}_k^a=\left(\begin{array}{c}{\mathit{z}}_{k,a}\\ {\mathit{z}}_{k,a}\\ {\mathit{z}}_{k,a}\end{array}\right),{\mathit{h}}_k^a\left({\mathit{x}}_k\right)=\left(\begin{array}{c}{\mathit{h}}_{k,a}^{\mathit{p}}\\ {\mathit{h}}_{k,a}^{\dot{\mathit{p}}}\\ {\mathit{h}}_{k,a}^{\ddot{\mathit{p}}}\end{array}\right).$$

(15)

The corresponding Jacobian matrix is written as

$$H_{k,a}\equiv \frac{\partial {\mathit{h}}_k^a}{\partial \mathit{x}}=\left(\begin{array}{cccccc}\frac{R{\left(\mathit{\theta }\right)}^T}{\Delta t^2}& {\mathbf{0}}_3& {\mathbf{0}}_3& \frac{\partial {\mathit{h}}_{k,a}^{\mathit{p}}}{\partial {\mathit{\theta }}_k}& \frac{\partial {\mathit{h}}_{k,a}^{\ddot{\mathit{p}}}}{\partial {\mathit{\omega }}_k^B}-\frac{C\left({\mathit{r}}^B\right)}{\Delta t}& {\mathbf{0}}_3\\ {\mathbf{0}}_3& \frac{R{\left(\mathit{\theta }\right)}^T}{\Delta t}& {\mathbf{0}}_3& \frac{\partial {\mathit{h}}_{k,a}^{\dot{\mathit{p}}}}{\partial {\mathit{\theta }}_k}& \frac{\partial {\mathit{h}}_{k,a}^{\ddot{\mathit{p}}}}{\partial {\mathit{\omega }}_k^B}-\frac{C\left({\mathit{r}}^B\right)}{\Delta t}& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& R{\left(\mathit{\theta }\right)}^T& \frac{\partial {\mathit{h}}_{k,a}^{\ddot{\mathit{p}}}}{\partial {\mathit{\theta }}_k}& \frac{\partial {\mathit{h}}_{k,a}^{\ddot{\mathit{p}}}}{\partial {\mathit{\omega }}_k^B}& -C\left({\mathit{r}}^B\right)\end{array}\right),$$

(16)

where $C(·)$ is the skew symmetric matrix in Equation (5) and

$$\begin{array}{cc}\hfill \frac{\partial {\mathit{h}}_{k,a}^{\mathit{p}}}{\partial {\mathit{\theta }}_k}& =\frac{\partial R{\left(\mathit{\theta }\right)}^T\frac{{\delta }^2{\mathit{p}}_k}{\Delta t^2}}{\partial \mathit{\theta }},\frac{\partial {\mathit{h}}_{k,a}^{\dot{\mathit{p}}}}{\partial {\mathit{\theta }}_k}=\frac{\partial R{\left(\mathit{\theta }\right)}^T\frac{\delta {\dot{\mathit{p}}}_k}{\Delta t}}{\partial \mathit{\theta }},\hfill \\ \hfill \frac{\partial {\mathit{h}}_{k,a}^{\ddot{\mathit{p}}}}{\partial {\mathit{\theta }}_k}& =\frac{\partial R{\left(\mathit{\theta }\right)}^T{\mathit{p}}_k}{\partial \mathit{\theta }},\hfill \\ \hfill \frac{\partial {\mathit{h}}_{k,a}^{\ddot{\mathit{p}}}}{\partial {\mathit{\omega }}_k^B}& =-C({\mathit{\omega }}_k^B\times {\mathit{r}}^B)-C\left({\mathit{\omega }}_k^B\right)C\left({\mathit{r}}^B\right),\hfill \end{array}$$

where $\delta$ represents the finite difference operation and ${\delta }^2{\mathit{p}}_k={\mathit{p}}_k-2{\mathit{p}}_{k-1}+{\mathit{p}}_{k-2}$ and $\delta {\dot{\mathit{p}}}_k={\dot{\mathit{p}}}_k-{\dot{\mathit{p}}}_{k-1}$.

3.3.2. Sensor Models of Gyroscope

Because the gyroscope measures the angular velocity, the proposed approach constructs two sensor models, each outputting the angular velocity and the sensor suite orientation:

$$\begin{array}{cc}\hfill {\mathit{z}}_{k,g}& ={\mathit{h}}_{k,g}^{\mathit{\omega }}({\mathit{x}}_k,\mathit{v})\equiv {\mathit{\omega }}_k^B+{\mathit{v}}_g,\hfill \\ \hfill {\mathit{z}}_{k,g}& ={\mathit{h}}_{k,g}^{\mathit{\theta }}({\mathit{x}}_k,\mathit{v})\equiv E{\left({\mathit{\theta }}_k\right)}^{-1}\frac{{\mathit{\theta }}_k-{\mathit{\theta }}_{k-1}}{\Delta t}+{\mathit{v}}_g,\hfill \end{array}$$

(17)

where ${\mathit{v}}_g$ is the gyroscope measurement noise, and the inverse of the Euler angle rates matrix is given by

$$E{\left(\mathit{\theta }\right)}^{-1}=\left(\begin{array}{ccc}1& 0& -sin\theta \\ 0& cos\varphi & sin\varphi cos\theta \\ 0& -sin\varphi & cos\varphi cos\theta \end{array}\right).$$

Let the measurements and the sensor models be described collectively again:

$${\mathit{z}}_k^g=\left(\begin{array}{c}{\mathit{z}}_{k,g}\\ {\mathit{z}}_{k,g}\end{array}\right),{\mathit{h}}_k^g\left({\mathit{x}}_k\right)=\left(\begin{array}{c}{\mathit{h}}_{k,g}^{\mathit{\theta }}\\ {\mathit{h}}_{k,g}^{\mathit{\omega }}\end{array}\right).$$

(18)

The Jacobian matrix is given by

$$H_{k,g}\equiv \frac{\partial {\mathit{h}}_k^g}{\partial \mathit{x}}=\left(\begin{array}{cccccc}{\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& \frac{E^{\prime }\left(\delta {\mathit{\theta }}_k\right)+\mathit{E}{\left({\mathit{\theta }}_k\right)}^{-1}}{\Delta t}& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathit{I}}_3& {\mathbf{0}}_3\end{array}\right),$$

(19)

with

$$E^{\prime }\left(\mathit{v}\right)=\left(\begin{array}{ccc}0& -v_{3}cos\theta & 0\\ -v_{2}sin\varphi +v_{3}cos\varphi cos\theta & -v_{3}sin\varphi sin\theta & 0\\ -v_{2}cos\varphi -v_{3}sin\varphi cos\theta & -v_{3}cos\varphi sin\theta & 0\end{array}\right).$$

(20)

$\delta {\mathit{\theta }}_k={\mathit{\theta }}_k-{\mathit{\theta }}_{k-1}$ is the finite difference.

3.3.3. Sensor Models for Angular Acceleration

The derivation of the angular acceleration ${\dot{\mathit{\omega }}}_k^B$ in Equation (13) from accelerometers in Equations (3)–(5) additionally introduces sensor models for angular acceleration. They are associated with the angular acceleration, the angular velocity, and the orientation as

$$\begin{array}{cc}\hfill z_{k,\dot{\mathit{\omega }}}& ={\mathit{h}}_{k,\dot{\mathit{\omega }}}^{\dot{\mathit{\omega }}}\left(\mathit{x}\right)\equiv {\dot{\mathit{\omega }}}_k^B+{\mathit{v}}_{k,\dot{\mathit{\omega }}},\hfill \\ \hfill z_{k,\dot{\mathit{\omega }}}& ={\mathit{h}}_{k,\dot{\mathit{\omega }}}^{\mathit{\omega }}\left(\mathit{x}\right)\equiv \frac{{\mathit{\omega }}_k^B-{\mathit{\omega }}_{k-1}^B}{\Delta t}+{\mathit{v}}_{k,\dot{\mathit{\omega }}},\hfill \\ \hfill z_{k,\dot{\mathit{\omega }}}& ={\mathit{h}}_{k,\dot{\mathit{\omega }}}^{\mathit{\theta }}\left(\mathit{x}\right)\equiv E{\left(\mathit{\theta }\right)}^{-1}\frac{{\mathit{\theta }}_k-2{\mathit{\theta }}_{k-1}+{\mathit{\theta }}_{k-2}}{\Delta t^2}+{\mathit{v}}_{k,\dot{\mathit{\omega }}}.\hfill \end{array}$$

(21)

where ${\mathit{v}}_{k,\dot{\mathit{\omega }}}\sim \mathcal{N}(\mathbf{0},{\Sigma }_{\dot{\mathit{\omega }}})$ is a measurement noise for angular acceleration. Let the measurements and the sensor models be described collectively:

$${\mathit{z}}_k^{\dot{\mathit{\omega }}}=\left(\begin{array}{c}{\mathit{z}}_{k,\dot{\mathit{\omega }}}\\ {\mathit{z}}_{k,\dot{\mathit{\omega }}}\\ {\mathit{z}}_{k,\dot{\mathit{\omega }}}\end{array}\right),{\mathit{h}}_k^{\dot{\mathit{\omega }}}\left({\mathit{x}}_k\right)=\left(\begin{array}{c}{\mathit{h}}_{k,\dot{\mathit{\omega }}}^{\mathit{\theta }}\\ {\mathit{h}}_{k,\dot{\mathit{\omega }}}^{\mathit{\omega }}\\ {\mathit{h}}_{k,\dot{\mathit{\omega }}}^{\dot{\mathit{\omega }}}\end{array}\right).$$

(22)

The Jacobian matrix is obtained by

$$H_{k,\dot{\mathit{\omega }}}\equiv \frac{\partial {\mathit{h}}_k^{\dot{\mathit{\omega }}}\left({\mathit{x}}_k\right)}{\partial \mathit{x}}=\left(\begin{array}{cccccc}{\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& \frac{E^{\prime }\left({\delta }^2{\mathit{\theta }}_k\right)+E{\left({\mathit{\theta }}_k\right)}^{-1}}{\Delta t^2}& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& \frac{{\mathit{I}}_3}{\Delta t}& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathit{I}}_3\end{array}\right)$$

(23)

with $E^{\prime }(·)$ being defined in Equation (20) and ${\delta }^2{\mathit{\theta }}_k={\mathit{\theta }}_k-2{\mathit{\theta }}_{k-1}+{\mathit{\theta }}_{k-2}$.

While the sensor models have been constructed, the remaining modeling process for angular acceleration is the derivation of the covariance ${\Sigma }_{\dot{\mathit{\omega }}}$ for angular acceleration from a pair of IMUs. We expand the right-hand side of Equation (4) by considering noise on top of the measurement of acceleration and angular velocity:

$$\begin{array}{cc}\hfill \mathit{b}+{\mathit{v}}_{\mathit{b}}=& -({\mathit{a}}_2-{\mathit{a}}_1)+\frac{1}{2}{\mathit{\omega }}_2\times ({\mathit{\omega }}_2\times \mathit{r})+\frac{1}{2}{\mathit{\omega }}_1\times ({\mathit{\omega }}_1\times \mathit{r})\hfill \\ \hfill & +{\mathit{v}}_{{\mathit{a}}_2}+\frac{1}{2}{\mathit{\omega }}_2\times ({\mathit{v}}_{{\mathit{\omega }}_2}\times \mathit{r})+\frac{1}{2}{\mathit{v}}_{{\mathit{\omega }}_2}\times ({\mathit{\omega }}_2\times \mathit{r})\hfill \\ \hfill & +{\mathit{v}}_{{\mathit{a}}_1}+\frac{1}{2}{\mathit{\omega }}_1\times ({\mathit{v}}_{{\mathit{\omega }}_1}\times \mathit{r})+\frac{1}{2}{\mathit{v}}_{{\mathit{\omega }}_1}\times ({\mathit{\omega }}_1\times \mathit{r})\hfill \\ \hfill & +\frac{1}{2}{\mathit{v}}_{{\mathit{\omega }}_2}\times ({\mathit{v}}_{{\mathit{\omega }}_2}\times \mathit{r})+\frac{1}{2}{\mathit{v}}_{{\mathit{\omega }}_1}\times ({\mathit{v}}_{{\mathit{\omega }}_1}\times \mathit{r}),\hfill \end{array}$$

where ${\mathit{v}}_{\mathit{b}}$ is the noise on vector $\mathit{b}$, ${\mathit{v}}_{{\mathit{a}}_1}$ and ${\mathit{v}}_{{\mathit{a}}_1}$ are the measurement noise of the accelerometer, and ${\mathit{v}}_{{\mathit{\omega }}_1}$ and ${\mathit{v}}_{{\mathit{\omega }}_2}$ are that of the gyroscope. By neglecting the smaller quadratic noise terms above, the covariance can be computed as

$${\Sigma }_{\mathit{b}}\approx {\Sigma }_{{\mathit{a}}_1}+{\Sigma }_{{\mathit{a}}_2}+\frac{1}{2}{\tilde{C}}_1{\Sigma }_{{\mathit{\omega }}_1}{\tilde{C}}_1^T+\frac{1}{2}{\tilde{C}}_2{\Sigma }_{{\mathit{\omega }}_2}{\tilde{C}}_2^T,$$

(24)

where

$$\begin{array}{cc}\hfill {\tilde{C}}_1& =C({\mathit{\omega }}_1\times \mathit{r})+C\left({\mathit{\omega }}_1\right)C\left(\mathit{r}\right),\hfill \\ \hfill {\tilde{C}}_2& =C({\mathit{\omega }}_2\times \mathit{r})+C\left({\mathit{\omega }}_2\right)C\left(\mathit{r}\right).\hfill \end{array}$$

$C(·)$ is the cross-product matrix defined in Equation (5).

3.3.4. Sensor Model of Downward Camera

Since the downward camera ultimately derives its pose with respect to the global frame, the sensor model associates the observations with the pose of the sensor suite:

$$\begin{array}{cc}\hfill {\mathit{z}}_{k,\mathit{p}}& ={\mathit{h}}_{k,\mathit{p}}({\mathit{x}}_k,{\mathit{v}}_{k,\mathit{p}})\equiv {\mathit{p}}_k+R{\left({\mathit{\theta }}_k\right)}^T{\mathit{r}}_c^B+{\mathit{v}}_{k,\mathit{p}},\hfill \\ \hfill {\mathit{z}}_{k,\mathit{\theta }}& ={\mathit{h}}_{k,\mathit{\theta }}({\mathit{x}}_k,{\mathit{v}}_{k,\mathit{\theta }})\equiv {\mathit{\theta }}_k+{\mathit{v}}_{k,\mathit{\theta }},\hfill \end{array}$$

(25)

where ${\mathit{r}}_c^B$ is the coordinate of the downward camera in the body frame. Let the measurements and the sensor models be described collectively:

$${\mathit{z}}_{k,c}=\left(\begin{array}{c}{\mathit{p}}_{k,c}\\ {\mathit{\theta }}_{k,c}\end{array}\right),{\mathit{h}}_{k,c}\left({\mathit{x}}_k\right)\equiv \left(\begin{array}{c}{\mathit{p}}_k+R{\left({\mathit{\theta }}_k\right)}^T{\mathit{r}}_c^B\\ {\mathit{\theta }}_k\end{array}\right)+{\mathit{v}}_{k,c},$$

(26)

The Jacobian of the downward camera model is given by

$$H_{k,c}\equiv \frac{\partial {\mathit{h}}_{k,c}}{\partial \mathit{x}}=\left(\begin{array}{cccccc}{\mathit{I}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& \frac{\partial R{\left(\mathit{\theta }\right)}^T{\mathit{r}}_c^B}{\partial \mathit{\theta }}& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathit{I}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\end{array}\right).$$

(27)

3.4. EKF

The implementation of the EKF for the localization of the sensor suite is relatively straightforward once the motion model and all the sensor models have been developed. Given the motion model (11), the proposed approach predicts the mean and the covariance, ${\mathit{x}}_{k|k-1}$ and $P_{k|k-1}$, using its prior belief ${\mathit{x}}_{k-1|k-1}$ and $P_{k-1|k-1}$ as

$$\begin{array}{cc}\hfill {\mathit{x}}_{k|k-1}& =F_k{\mathit{x}}_{k-1|k-1},\hfill \\ \hfill P_{k|k-1}& =F_k{P}_{k-1|k-1}{F}_k^{\top }+V_k{\Sigma }_{k,\mathit{w}}{V}_k^{\top }.\hfill \end{array}$$

(28)

The sensor models are each given by a function of some state variables. Accordingly, the proposed approach corrects the estimation through the parallel KF where the correction of each state from each sensor is fused through summation [42]:

$$\begin{array}{cc}\hfill {\mathit{x}}_{k|k}& ={\mathit{x}}_{k|k-1}+\sum _{s\in \mathcal{S}}{K}_{k,s}\left({\mathit{z}}_{k,s}-{\mathit{h}}_{k,s}\left({\mathit{x}}_{k|k-1}\right)\right),\hfill \\ \hfill P_{k|k}^{-1}& =P_{k|k-1}^{-1}+\sum _{s\in \mathcal{S}}{H}_{k,s}^T{\Sigma }_{k,s}^{-1}{H}_{k,s},\hfill \end{array}$$

(29)

where $s\in \mathcal{S}$ is each sensor, and ${\Sigma }_{k,s}$ and $H_{k,s}$ are the covariance of measurement noise of sensor and the Jacobian matrix of the sensor model ${\mathit{h}}_{k,s}\left(\mathit{x}\right)$, respectively. Kalman gain for sensor s is given by

$$K_{k,s}=P_{k|k}{H}_{k,s}^T{\Sigma }_{k,s}^{-1}.$$

(30)

The great advantage of the proposed formulation is the simplicity. While the dimension of each correction is different, the Kalman gain acts not only as a scaling factor but also as a transformation matrix. All the correction terms are thus summed without loss of generality.

4. Sensor Suite Design and Installation

Figure 5a shows the design of the sensor suite with all sensors installed. The stereo cameras are installed on the sensor suite using an orientation-adjustable base to have a better view of the toeboard. The downward camera is installed on a leg welded on the sensor suite. A three-axis gyroscope and a three-axis accelerometer are equipped on each camera to estimate its position and orientation. The frame size is $560\mathrm{mm}\times 450\mathrm{mm}\times 130\mathrm{mm}$. The size enables the sensor suite to be installed on the mounting bolt holes of the front seat without modifications to the test vehicle structure, as shown in Figure 5b.

The sensor specifications are shown in

Table 1. Parameters of sensors.

Inertial sensors sampling rate	20,000 Hz
Accelerometer noise	$0.01{\mathrm{m}/\mathrm{s}}^2\left(1\sigma \right)$
Accelerometer range	$2000\mathrm{g}$
Gyroscope noise noise	$0.{01}^{\circ }/\mathrm{s}\left(1\sigma \right)$
Gyroscope range	18,000${}^{\circ }/\mathrm{s}$
Camera sampling rate	$1000\mathrm{Hz}$
Camera resolution	$1024\times 1024\mathrm{pixels}$
Downward camera to the ground	$95\mathrm{mm}$
Target toeboard size	$500\mathrm{mm}\times 350\mathrm{mm}$

. Since the the crash of vehicle test typically lasts 100∼200 ms, high-speed sensors with a wide measurement range are selected.

The sensor suite was designed and tested in a laboratory environment. It was later tested at the crash test center of Honda Research & Development Americas, Inc. (Ohio, USA), in a full-size passenger car. Figure 6a,b show the side and top view of the sensor suite installed on the test vehicle. The stereo camera was placed $400\mathrm{mm}$ away from the toeboard so both cameras could have a full view of it. A hole with size $150\mathrm{mm}\times 150\mathrm{mm}$ was opened on the vehicle floor to install the downward camera, as shown in Figure 6b,c. The downward camera was about $95\mathrm{mm}$ above the ground and had a clear view of the checkerboard pattern. Additional installations of IMU are shown in Figure 6c,d.

5. Experimental Results

This section first demonstrates the localization of the sensor suite using the proposed EKF framework in a simulated environment. Then, results of a real car crash test are followed.

5.1. Sensor Suite Localization in Simulated Environment

Consider the following case where the center of sensor suite undergoes simple linear and angular motion in the global frame as

$$\begin{array}{cc}\hfill \mathit{a}& ={\left(\begin{array}{ccc}-{\left(2\pi f\right)}^{2}sin\left(2\pi ft\right),& 0,& 0\end{array}\right)}^T,\hfill \\ \hfill \mathit{\omega }& ={\left(\begin{array}{ccc}-2\pi f_w\left(cos\left(2\pi f_{w}t\right)-1\right),& 0,& 0\end{array}\right)}^T,\hfill \end{array}$$

with angular acceleration

$$\dot{\mathit{\omega }}={\left(\begin{array}{ccc}{\left(2\pi f_w\right)}^{2}sin{\left(2\pi f_{w}t\right)}^T,& 0,& 0\end{array}\right)}^T.$$

The measurement of the accelerometer and gyroscope in the body frame shall be

$$\begin{array}{cc}\hfill {\mathit{a}}_{IMU}^B& ={\mathit{a}}_c^B+{\dot{\mathit{\omega }}}^B\times {\mathit{r}}^B+{\mathit{\omega }}^B\times ({\mathit{\omega }}^B\times {\mathit{r}}^B)+{\mathit{v}}_a,\hfill \\ \hfill {\mathit{\omega }}^B& =-\mathit{\omega }+{\mathit{v}}_g\hfill \end{array}$$

(31)

where ${\mathit{a}}_c^B$ and ${\dot{\mathit{\omega }}}^B$ are measurements at sensor suite center as

$${\mathit{a}}_c^B=-\mathit{a},{\dot{\mathit{\omega }}}^B=-\dot{\mathit{\omega }}$$

and ${\mathit{v}}_g\sim \mathcal{N}(0,{\sigma }_g)$ and ${\mathit{v}}_a\sim \mathcal{N}(0,{\sigma }_a)$ model the sensor noise of accelerometer and gyroscope. The pose of downward camera shall be provided by

$$\begin{array}{cc}\hfill {\mathit{p}}_{C_D}& =\mathit{p}+R\left(\mathit{\theta }\right){\mathit{r}}_{C_D}+{\mathit{v}}_{c,\mathit{p}}\hfill \\ \hfill {\mathit{\theta }}_{C_D}& =\mathit{\theta }+{\mathit{v}}_{c,\mathit{\theta }},\hfill \end{array}$$

(32)

where the position and orientation of the sensor suite are

$$\begin{array}{cc}\hfill \mathbf{p}& ={\left(\begin{array}{ccc}sin\left(2\pi ft\right)-2\pi ft-2\pi ,& 0,& 0\end{array}\right)}^T,\hfill \\ \hfill \mathit{\theta }& ={\left(\begin{array}{ccc}-sin\left(2\pi f_{w}t\right)+2\pi f_{w}t+2\pi ,& 0,& 0\end{array}\right)}^T.\hfill \end{array}$$

Here, $R\left(\mathit{\theta }\right)$ is the rotation matrix given Euler angle $\mathit{\theta }$, and ${\mathit{r}}_{C_D}^B$ is the position of downward camera in the body frame. Sensor noise of camera ${\mathit{v}}_c\sim \mathcal{N}(0,{\sigma }_c)$ is Gaussian. IMU data are available between $-0.1s\sim 0.3s$ while camera pose information is available between $-0.01s\sim 0.2s$, to be consistent with the real car crash test.

By letting $f=f_w=10$, the angular acceleration can be recovered from the measurement of accelerometers and gyroscope, as illustrated in Figure 7a. This angular acceleration follows the prescribed ${\dot{\mathit{\omega }}}^B$. The amplitude in the x-direction is close to the given amplitude ${\left(2\pi f_w\right)}^2$. The other two directions (${\dot{\omega }}_y$ and ${\dot{\omega }}_z$) are zero-meaned. The estimated velocity is shown in Figure 7b. Linear velocity and angular velocity at x-direction follow the cosine wave, while motion in other directions is trivial. The displacement and roll angles are shown in Figure 7c,d. Figure 7e illustrates the motion of the sensor suite in space, where rectangular panels illustrate the position with a normal vector for the orientation.

Differential entropy of the estimation is plotted in Figure 8, which is given by

$$h\left(\mathit{x}\right)=\frac{1}{2}log\left[{\left(2\pi e\right)}^n\left|{\Sigma }_{k|k}\right|\right]$$

for multivariate Gaussian distribution considered here. The entropy shows that the downward camera pose significantly improves the certainty of the estimation, as shown in Figure 8.

To test the limits of the proposed EKF framework, we vary noise level by ${\sigma }_a$, ${\sigma }_g$, or ${\sigma }_c$ individually while keeping the other two at a low level, $0.1\%$ of the maximum value, to be specific. For each combination, 100 tests were performed and the statistics of maximum error of the position $max\parallel {\mathit{p}}_{k|k}-{\mathit{p}}_k\parallel$ are plotted in Figure 9, where ${\mathit{p}}_{k|k}$ is the estimation and ${\mathit{p}}_k$ is the ground truth at step k. The figure shows that the proposed EKF-based estimation is accurate when noise is typically small, say, less than $1\%$. Then, error can grow significantly when the noise of accelerometers and gyroscope further increases. Note that dead-reckoning error comes from measurements of accelerometer and gyroscope; therefore, noise on the downward camera pose may not significantly influence the estimation if IMU measurements are not significantly noisy.

Figure 7. Results of sensor suite localization using EKF framework in a simulated environment. (a) Angular acceleration; (b) linear and angular velocity of the camera fixture; (c) Estimated displacement of the camera fixture; (d) estimated roll angle of the camera fixture; (e) trajectory of the camera fixture in space.

Figure 7. Results of sensor suite localization using EKF framework in a simulated environment. (a) Angular acceleration; (b) linear and angular velocity of the camera fixture; (c) Estimated displacement of the camera fixture; (d) estimated roll angle of the camera fixture; (e) trajectory of the camera fixture in space.

Figure 8. Entropy of sensor suite localization in simulated environment.

Figure 8. Entropy of sensor suite localization in simulated environment.

5.2. Sensor Suite Localization in 56 km/h Frontal Barrier Car Crash Test

5.2.1. Pose Estimation of Downward Camera

Figure 10 shows the results of pose detection of the downward camera using the computer vision technique. Figure 10a shows the checkerboard pattern detected, while Figure 10b shows the features matched between two consecutive images for the purpose of localizing the downward camera. With pixel coordinates in Figure 10a and their global frame coordinates through Equation (7), camera pose is computed and presented in Figure 10c for position and orientation in Figure 10d.

5.2.2. Localization of Sensor Suite

For the localization purpose, angular acceleration is first computed by using IMU measurements and is plotted in Figure 11a. The results show that the pitch motion (rotation along y-axes), related to ${\dot{\omega }}_y$, is significantly larger than the other two directions, which is consistent with onsite observation. The state of the sensor suite is then estimated through the proposed EKF framework with the computed pose of downward camera, angular acceleration, and IMU measurements. The results are shown in the rest of Figure 11.

Figure 11b shows the estimated velocity. The frontal crash test has an initial speed of $15.6\mathrm{m}/\mathrm{s}$ ($56\mathrm{km}/\mathrm{h}$). The results show that, after the crash, the vehicle bounced back and moved powerlessly at a speed around $-3.8\mathrm{m}/\mathrm{s}$. Motion in the y- and z-direction is much smaller than the x-direction. This is consistent with the onsite measurement.

Figure 11c shows the position estimated for the sensor suite. Instead of the center, the position of the downward camera is plotted so that results from Section 5.2.1 can be compared. The compression of structures, the distance moved in x-direction after the crash, is about $700\mathrm{mm}$ from the figure. It conforms to the onsite measurement. The figure also shows that the vehicle bounced up about $50\mathrm{mm}$ after the crash (from $33\mathrm{ms}$ to $77\mathrm{ms}$), and the motion in the z-direction was relatively small compared to the other two directions.

Figure 11d shows the pitch motion during the crash. The estimation shows that the maximum pitch angle is around $8^{\circ }$ during the crash. For comparison, we also include the pitching angles measured by the downward camera and the gyroscope installed at the center of the designed sensor suite. The camera measurement is subject to the oscillation of the frame and shows strong oscillations throughout the in-crash measurement. Meanwhile, the integration of gyroscope measurement is subject to the dead-reckoning errors and it is hard to measure the precise pitching angle. Therefore, measurement based on a single sensor may be less accurate than the state estimation technique that fuses multiple sensor measurements. The motion of the sensor suite during the crash is illustrated in Figure 11e, where rectangular panels represent its position and orientation at every other $5\mathrm{ms}$. On each panel, the normal vector is plotted and scaled by velocity $v_x$ to illustrate the orientation and velocity of the sensor suite during the crash test.

Figure 11. Frontal barrier localization result. (a) Displacement of the camera fixture. (b) Pitch angle of the camera fixture. (c) Localization of camera fixture in the global frame.

Figure 11. Frontal barrier localization result. (a) Displacement of the camera fixture. (b) Pitch angle of the camera fixture. (c) Localization of camera fixture in the global frame.

6. Conclusions and Ongoing Work

This paper presented a technique to localize a stereo camera for in-crash toeboard deformation measurement. For localizing purposes, we designed a sensor suite with IMU sensors and cameras. This technique recursively estimates the pose of the sensor suite to where the stereo camera is attached. Using an EKF-based framework, the state, including state mean and covariance, is predicted using a motion model and current state. The prediction is then corrected through proposed sensor models and measurements of IMUs and the camera. The state estimation technique can remove the dead-reckoning errors of IMU sensors. Compared to the classical measurement-based technique, the state estimation can remove the measurement error of a single sensor incurred by the excessive oscillation of the sensor suite during a crash test. The proposed technique was first verified in a simulated environment, in which the estimation matched well with the ground truth, and the estimation was reliable given moderate sensor noise. The real crash data were analyzed and provided localization results of the sensor suite as well as of the stereo camera.

This paper mainly focuses on the localization of a sensor suite for toeboard deformation measurement, where the latter is not touched on in this paper but will be reported in future work. We noticed that cameras underwent relative motion to the sensor suite in the real crash test. This may be due to the structural flexibility, and further investigation of the influence of structure flexibility may provide more accurate localization results.