The task is to define the functional dependence for the transformation of the state vector xn into the components of the sensor fusion observation vector zn. If the observation noise is included, this transformation can be written as
where . The observations consist of two separate components, the IMU components and the photogrammetric components, which must be fused together to provide an asynchronous stream of observation data.
The IMU package outputs data from three orthogonal accelerometers and three orthogonal rate gyroscopes. The IMU observation vector at time tn can be written as
where zx,n, zy,n, and zz,n are the accelerations (in G's) measured by the x-forward, y-right, and z-down accelerometers, respectively, and zϕ,n, zθ,n and zϕ,n are rotation rates (in degrees) measured by the roll (counterclockwise about the x-axis), pitch (counterclockwise about the y-axis), and yaw (counterclockwise about the z-axis) rate gyroscopes, respectively. Note that since the IMU package is attached to the rigid body, it rotates as the body rotates so that the measurements ...