Assignment #4
State estimation, Kalman filtering quaternions and 3-D dynamics.
Observe the noisy markers on an ‘alien artifact’ tumbling freely in space.  Use low-pass filtering and Kalman filtering to estimate the state of the object from the noisy measurements of markers on the artifact.  Use this information to control a ‘lander’ and make it dock with the artifact.
Monday, April 5, 2010
Low-pass Filtering
Part 1.
Use a low-pass filter to clean up the noise on the markers.  Optimize the filter parameters and use the data to estimate the artifact’s state.
The noise present in the marker data has a much higher frequency than the trends we’re concerned about, so low-pass filtering will work relatively well.  Using just a rolling-window average of various sizes I was able to tune a filter that achieved minimum average error across the markers.  Qualitatively, this corresponds to a window size that significantly smooths the signal, without inducing too much lag.
A filter window size of 20 samples was optimal with these criteria, and showed significant improvement to the readings when applied to the marker data.  See figures below.
With this filtered data I then estimated the center of mass and pose of the artifact.  To estimate the center of mass, I set up a function that compared the distance of each marker to an estimated center of mass and used Nelder-Mead optimization to find a point that minimized the change in each distance over time.  This quickly produced an estimate that was within 1% of the actual marker positions.
Using this center of mass estimate, I then used SVD to determine a rotation matrix between each time step and a base pose.  Below is the elements of this estimated rotation matrix plotted against the conversion of the actual quaternion pose into a rotation matrix.  In order to estimate velocity, the estimated rotation matrix can be differentiated about each principle axis.

Kalman Filtering
Part 1.
The next step is to implement an extended Kalman filter to estimate the state of the artifact at each time step.  To do this the state space of the dynamics model was augmented to include:
    Center of mass ( xyz coordinates in World frame )
    Principle moments of inertia ( in Artifact frame )
    Marker locations (xyz vectors in Artifact frame )
    Angular velocity (in Artifact frame )
    Quaternion Pose (in World frame )
The center of mass was estimated by taking the average of all the points at the first (unfiltered) time step.  The pose was chosen arbitrarily to be [.5 .5 .5 .5] and marker locations relative to the center of mass were calculated based on this pose and the first time step measurements.  The angular velocities were assumed to be [0 0 0] and the moments of inertia to be equal.
The values for the process noise (Q) and the measurement noise (R) were chosen through experimentation.  The value for R was set to .333, based on the estimated variance from the noisy readings.  To simplify things, a uniform diagonal Q was chosen for all state variables and was set at .000001.  This provided a good balance between accurate and fast convergence of state parameters.
The plots below show the convergence of the moments of inertia and angular momentum to the correct values from the simulation.  Note the that colors denoting the axis for each velocity do not match on the velocity plot.  This is because the filter converged to a set of parameters that were equivalent and orthogonal to the ‘true’ values provided in the simulation.
This filter implementation ran fast enough to process the data in ‘real time’, even in MATLAB.  The video below shows the filter converging to data from the simulation.  The blue circles are the true marker locations, the green circles are the noisy measurements that are used in filtering, and the red crosses are the filtered estimates.  The blue and red star are the actual and estimated centers of mass, respectively.
This is a transient Kalman filter.  Because of the continued re-linearization of the dynamics about the current state, the covariance and Kalman matrices are constantly changing.  The covariance matrix becomes roughly constant over the 10000 timesteps of the simulation, but the Kalman gains settle into a roughly cyclic pattern as the artifact tumbles due to its non-linear dynamics.
These gain plots were used to attempt to create a steady-state Kalman filter.  Due to the fact that the system is non-linear, this is a bad idea.  I tried a couple different methods that used the gains towards the end of the simulation, both averaging over an extended period and trying consistent gains sampled at different points in time.  The filter always diverged within ~20 seconds, and was accurate only briefly, if ever.
3D Dynamics
Part 3.
The tumbling of the artifact has different characteristics, depending on the initial angular velocities and the principle moments of inertia for each corresponding velocity.  In general the object will be more stable if velocities are greatest around axes with low moments of inertia, and the least stable if the greatest velocities are about the axes with high moments of inertia.  This is because of the way velocities on orthogonal axes create accelerations on each other:
    wx_new = wx - (Ts)*(wy*Iz*wz - wz*Iy*wy)/Ix;
    wy_new = wy - (Ts)*(wz*Ix*wx - wx*Iz*wz)/Iy;
    wz_new = wz - (Ts)*(wx*Iy*wy - wy*Ix*wx)/Iz;
By adjusting the starting velocities of the artifact, I could generate different characteristics in the dynamics.  In the case where the initial angular velocities are highest about the axis with the lowest moment of inertia, the artifact only transfers velocities to the other axes very slowly (see below):
However, if the velocity is highest on the middle principle moment of inertia, the velocity varies quite quickly between the other axes, and creates a near-random tumbling.  Both of these plots were for velocities of 1 rad/s about one axis, and .1 rad/s about the other two.  In the above plot, the high velocity axis was the lowest moment of inertia.  Below, it was the middle moment.
Part 4.
By implementing the extended Kalman filter from part 2 in C, I was able to generate a state estimate to be used by the lander.  In this case I created an imaginary point that was just a scaled out projection of “marker 1” in the state vector for the filter.  This served as the xyz target for docking, while the artifacts pose and velocity served directly as the pose and angular velocity of the lander.  By calculating the estimated future position marker at the next time step I was able to also add a desired velocity to the force portion of the controller.  
The performance can be seen in the video below.  The lander moves the the point, a constant distance away from the artifact.  The lander’s point of view is shown on the right side of the screen.  The red points are the estimated marker positions, and the green points are the actual noisy measurements.  The artifact stays close to stationary to the lander’s point of view, due to being able to close a loop on desired position and velocity.