In this assignment, we consider a scenario in which a spacecraft (lander) is observing an unforced tumbling alien artifact in space. We are observing a set of feature points (markers) on the artifact, and the data association problem is solved so that a noisy/corrupted but unambiguous 3-D position stream relative to the lander is available for each marker. We do not know where these points lie on the artifact nor anything about its geometric or inertial parameters, however we seek to track its position, velocity, orientation, and angular velocity so that we may eventually servo the lander to land on it.
First, we consider merely low-pass filtering the marker data to estimate the desired quantities, without using an actual predictive dynamics model.
Towards this end, I first implement said low-pass filter, in the space of the relative 3-D positions directly provided. Somewhat arbitrarily, I chose a first-order Butterworth filter, implemented as a first-order linear filter:
where A and B are 1x2 matrices produced by the matlab Butterworth filter generator that is parameterized by a single parameter -- the cuttoff frequency -- of which I found the optimal value by an optimization using matlab that ran the simulation for a given value of this parameter and computed a cost represented by the mean sum-squared-error in filtered marker locations relative to the actual ones. The procedure for performing this may be found in lowpass_filter_optimization/. The derivative of these marker locations (their velocity in lander coordinates) is estimated directly as the finite difference between positions at adjacent timesteps. Because this velocity may be rather noisy, I run it through a second low pass Butterworth filter, whose cut-off frequency is optimized using the same matlab script so that the best parameters for the second filter are found given the best parameters for the first filter. Quite possibly, a linear observer directly estimating velocity would be more appropriate, but I wasn't able to formulate one to my satisfaction, and this has proven to work reasonably well.
Given a stream of smooth, reasonably accurate 3-D marker positions relative to the lander, we may begin to estimate the artifact's pose and motion. Since we don't know where the aliens chose to place their origin and coordinate axes, we must choose these arbitrarily. I do this by choosing the mean position of the marker locations as the origin and building an orthogonal basis from the locations of the first three markers relative to this origin. While this will provide a reasonable coordinate system in any event, I chose this deliberately so that the resulting coordinate system is conveniently coincident with the actual local coordinate system of the artifact in the simulation, allowing me to compare estimated values easily without having to apply the pose transform between my arbitrary and the actual simulated coordinate systems.
Because this mean and orthogonal basis are computed the same way at each time step, the mean may be used as the position of the artifact, and the basis provides its orientation. In world coordinates, this position and orientation (represented as a rotation matrix) may be written as:
since the mean and basis were computed in coordinates relative to the lander.
Next, noting that the relationship:
must hold for each marker i, I solve the overconstrained (due to the noise in observations) linear system:
where A contains 3 rows for each marker (24 rows total, in this case) to solve for the body velocity v and angular velocity omega (both in alien local coordinates). The body velocity is again transformed into world coordinates by transforming by the estimated pose of the alien, and the angular velocity is maintained in local body coordinates as is conventional.
Plots from a typical run:
 Better PDF version here |
 Better PDF version here |
 Better PDF version here |
 Better PDF version here |
 Better PDF version here |
 Better PDF version here |
 Better PDF version here |
 Better PDF version here |
While the algorithm should work fine for alternative choices of alien local coordinate system and a large variety of initial conditions, by virtue of being a low-pass filter, if the motion were to speed up considerably, tracking quality would be reduced, as the cut-off frequencies were chosen to be optimal for motion of approximately this magnitude. Further, this will exhibit common properties of low-pass filters, for example that as the fall-off increases the phase offset of the filtered output will increase, which can result in large instantaneous error even though the form of the signal may be very close.
Implementing a Kalman filter to perform the same task will allow us to use a predictive model that considers likelihoods of states and measurements to overcome noise rather than just smoothing. Towards this end, we consider implementing a typical Extended Kalman Filter (EKF).
First, we model the state of the artifact:
which includes its position (world coordinates), orientation (world coordinates), velocity (world coordinates), angular velocity (alien local coordinates), the six unique elements of the artifact inertia matrix I (given that it is symmetric), and finally the relative positions of all the markers observed (alien local coordinates). The derivative of the state is then:
which in this case is completely a function of the state given that we are assuming the artifact is tumbling freely in space, unforced. The position derivative was already a state variable, the derivative of a quaternion may be found by taking half the product (using quaternion multiplication) of the current orientation with the quaternion formed by using 0 as the scalar component and the current angular velocity as the direction components, the acceleration is assumed to be zero (constant velocity model), the derivative of angular velocity is as provided in the dynamics handout code and as derived in class, and finally it is assumed that the inertia matrix and the relative marker locations are constant.
An EKF for an unforced system takes the typical form:
in which f is the process model whose jacobian with respect to state derivative is F, h is the measurement model whose jacobian with respect to state is H and which produces estimates z, P is the state estimate covariance, Q is process covariance, R is measurement covariance, and K is called the Kalman gain.
This form is the most common and is the time-varying model. Alternatively, we may use a steady-state Kalman filter, which is a simplified version of the above:
in which covariances are not tracked but rather a fixed optimal Kalman gain K_opt is used instead. This may be reasonable in cases in which the filter is starting with an estimate that it would have converged to and simply needs to continue tracking the system. The value P_opt is the state covariance the filter would have converged to after many iterations and may be called the steady-state covariance and technically fulfills the last relation noted above.
As for the actual implementation, the filter is initialized by running the low-pass filter described above for a short while, after which its best estimate of the artifact pose, velocity, and angular velocity are filled in. The inverse transform of the estimated artifact pose is applied to the most recent filtered observations of marker locations in world coordinates to get estimates of marker locations relative to the artifact. Finally, there are several options for generating an initial estimate of the inertia matrix. The first is to simply set it to the identity and hope that it converges to reasonable values, which may be far from diagonal if the physical principal axes do not align with the coordinate system chosen. A second I attempted was an optimization over observations made during the low-pass filter run. As the only way of which I am aware to observe the effect of the inertia matrix is via the relationship in f(x) above, I formulated this as an optimization of I to reduce the difference between the observed and predicted change in omega between one timestep and the next. This was implemented as a gradient search with derivatives computable because we are optimizing over a known function of observed values and the unknown I. The implementation of this may be found in I_solver.cpp. Unfortunately, as this function is bumpy due to noisy measurements and few measurements are available during this short initialization run, even with many random restarts the result this optimization converges to isn't very close to the correct answer.
Next, an initial covariance P must be chosen. As is typical, I choose this matrix to be diagonal, that values of which are my best guess of variances in the initial estimates. These are fairly arbitrarily chosen but based on observations of how well the initial estimates match the actual ones and are tuned by observing how well the relevant state values converge and whether the estimates are consistent (within several standard deviations provided by the running estimate covariance). Because the scale of the inertia matrix cannot be determined from the provided model (and potentially at all, from observing the artifact because we don't know its density?), I set its first value I00 to 1.0 and set the relevant element of P (and of course Q) to 0.0 to prevent the filter from changing it.
The linearization of the state from which A and H are computed was performed as much as possible in a matlab script derive_full_kalman.m and fed into a perl script to convert the result into executable C saved in phi.cxx (phi being the other common notation for A). Needless to say, with these many state variables and their nontrivial relationships, A is a large and hideous matrix. Along the way, I implemented progressively more complicated Kalman filters that for instance didn't estimate orientation or the inertia matrix or marker positions, and #defines are provided in the source to choose these smaller models.
The process model is implemented directly as trapezoidal integration of the state using the f(x) derivative function. I verified it to be representative of the sdfast results but slightly less accurate. The process noise matrix Q was also chosen to be a diagonal matrix, the values of which are again somewhat arbitrary. Following the suggestions provided in the literature, including Al Kelly's lengthy and famous technical report on positioning for mobile robots, I attempted to represent these as stemming from possible unmodeled motion. In particular, for instance, the variance on the change in velocity (modeled as none in this case) is set as the square of a practical bound on acceleration times the simulation time-step (hence, this is the square of a practical bound on change in velocity). What these bounds are, however, is of course less clear, and hence these values are subject to empirical tuning.
Finally, predicted observations (marker locations in world coordinates) are generated by transforming the estimated relative marker locations by the estimated artifact pose. Actual observations are made by taking the noisy observations provided and transforming by the known pose of the observing lander. The observation covariance matrix R is chosen to be diagonal with a consistent value set to the square of the known noise magnitude. An additional scale factor is provided to enable empirical tuning.
I implemented both time-varying and steady-state gain filters: the prior as described, and the latter which loads a saved steady state covariance that would be saved by a prior run of the time-varying filter and uses it in place of a continuously estimated covariance. I haven't the foggiest idea as to how to "plot" the [num states] x [num observation] K matrix (especially when each of those values is quite large). However, one would expect that a steady state filter with estimates starting near the correct value and using a converged covariance matrix to maintain approximately the same gain matrix that the time-varying filter converged to.
Given all this, we may finally run the filter. Doing so, I notice that the filter is much more sensitive than many others I have used in the past, in that only a very careful selection of initial, process, and measurement covariance values will prevent filter divergence. Divergence when it does occur appears to stem from the weak observability of the inertia matrix parameters and the deadly positive feedback that may occur for egregiously wrong estimates of these parameters. (I may also note that I am not enforcing that the inertia matrix maintain the positive definite property; I attempted to find a routine for doing this that I could simply call after each update step, but apparently the problem of finding the "nearest" p.s.d. matrix to a given one is a difficult problem, and those who implement solutions hoard their code.) I also somewhat blame the large number of parameters to estimate and the difficulty in generating accurate initial estimates. For an EKF with complicated models, it is my understanding that the impact of linearization error when further away from the true mean is more serious, so accurate initial estimates are even more vital.
For the purpose of parameter tuning, I again tried applying optimzation called from within matlab that attempts to estimate a cost that is roughly a sum squared error in the various parameters across a run. Because testing parameter values across large variations in magnitudes is necessary to explore the space well, this optimization effort was not particularly fruitful, as the builtin routines are not well suited for wide exploration (eg, they seek to minimize from a small initial simplex). This effort alone could occupy quite a lot of time and hence was not pursued heavily, however, the final values settled upon were optimized in this way.
Plots from a representative run are provided below.
 Better PDF version here |
 Better PDF version here |
 Better PDF version here | |
 Better PDF version here |
 Better PDF version here |
 Better PDF version here | |
 Better PDF version here |
 Better PDF version here |
 Better PDF version here | |
 Better PDF version here |
 Better PDF version here |
 Better PDF version here | |
The key observations I make from these is that the filter does a good job of tracking position and orientation, a reasonable job of tracking velocity, and a potentially tolerable job of tracking angular velocity. As I've hinted at previously, I believe this to largely be because though the inertia matrix evolves, it doesn't converge to a value very close to the true value, which causes an inability to accurately predict changes in angular velocity. However, it is pleasing to see that the filter is consistent, which is to say that the errors in the estimates lie within some small number of standard deviations (I here plot 3) as provided by the estimated state covariance. This implies that the filter is not overconfident -- a common mistake among naive implementors -- and thus that confidence in the resulting answer may be trusted.
We now consider what effect initial angular velocity has on the tumbling behavior of the artifact. "Tumbling" is a result of a changing angular velocity vector, particularly from changes in its direction. Recall again the relationship that dictates this phenomenon in an unforced scenario:
Assuming a sane inertia matrix, note that the case in which no change in omega occurs is when omega cross (I times omega) is zero, which may be if omega is zero (no rotation is occurring, so none will suddenly occur) or when omega is parallel to (I times omega). This will happen if I is some multiple of the identity matrix, which is roughly to say that masses are balanced along the coordinate axes and equal across the axes.
Likewise, the case when the greatest change in omega occurs is when omega cross (I times omega) is maximized, which will occur when omega is perpendicular to (I times omega), or in other words when I rotates (and potentially scales) omega to line in omega^{perp} (the set of vectors perpendicular to omega). In general I cannot be a rotation matrix (except for the null rotation) because of its symmetry, but it may be able to transform vectors into this set. Unfortunately, I have neither a concise representation of the set of such values of I nor an intuitive geometric explanation.
Finally, once we have estimated the pose and velocities of the artifact, we may consider thrusting the lander to meet the artifact.
The provided example controller essentially accomplishes this, however in order to keep things interesting, I extended it with a state machine that executes a deliberate approach. First, for a short while the lander only executes a servo on orientation so that its orientation is tracking that of the artifact. Then, it observes what face on the artifact is currently facing the lander, and (making some assumption that faces on the artifact are axis aligned, which they are here) it saves a vector in artifact coordinates along which it will make its approach. The lander first servos to a position a specified distance out along this vector while continuing to servo orientation. Then, a linear spline controller is executed to move from this position to a position a shorter distance out alon the approach vector, again while continuing to track servo orientation. This in effect executes a landing (modulo the distance parameters which may need to vary; in practice, a lander would study the geometry of the object on which it is landing and decide where the surface of the object lies relative to its estimated center of mass).
As for how the servos actually work, the position servo is of course the easiest. The error vector between current and desired location is a vector in world coordinates that may be transformed into local lander coordinates by applying its inverse orientation transform. This is now a desired motion vector in local coordinates, and since translational thrusters are available, these may be used to generate acceleration along this vector.
The position servo is somewhat more complicated but also easily enough understood. Applying the inverse orientation transform of the current lander orientation to the desired lander orientation provides a quaternion representing (surprisingly enough) the orientation transform between the two. Any such rotation may be represented in any of a number of ways, of which quaternions and rotation matrices are two. Rotation vectors, specifying an axis of rotation and an angle offset around said axis that implements the desired rotation, are another. Thus, converting the quaternion offset between current and desired orientation and then representing said as a rotation vector provides an axis about which one should generate an angular velocity as well as a magnitude. Given rotational thrusters on the lander, we may directly do exactly that.
I increased the proportional gains on both the servos from the provided values somewhat to provide better tracking, but I did not alter the derivative controller -- although we clearly would like to servo the translational and angular velocities of the lander to that of the artifact, my experiences implementing all the above indicates that we cannot estimate these derivatives with any particularly high degree of accuracy, so it seems safe to treat changes in artifact velocities as perturbations that must be tracked.
Source code may be found here.