This assignment explores Kalman filtering, 3D orientation, and 3d control.
Here is a simulation of a spacecraft (lander) and an alien artifact (alien). Your goal is to land the lander on the alien artifact. The alien artifact has some markings that you can measure the 3D location of relative to the lander. However, the measurements are noisy. See markers_noisy[][] in the code and the data file. There are two display programs. animate.c shows everything, and animate-lander.c shows dots only from the lander's viewpoint.
Part 1: Implement a simple low pass filter to clean up the marker data. What filter order and parameters work best? How do you use the filtered marker data to estimate the pose and angular velocity of the alien artifact at each instant in time?
Part 2: Design a Kalman Filter to estimate the pose and angular velocity of the alien artifact at each instant in time.
You will need to first estimate the marker locations relative to each other in a coordinate system rotating with the alien artifact. This will be part of your model of how the marker measurements are generated based on the alien artifact state. See dynamics.c for more info on the measurement model.
You will also need to estimate the moment of inertia matrix for the alien artifact in order to implement a process model. See dynamics.c for more info on the process model.
Google "quaternion kalman filter" to get info on how to handle a quaternion in a Kalman filter.
Now you are ready to implement the Kalman filter. Implement both a transient and a steady state version. What is the difference? Plot "gains" of the transient Kalman filter vs. time. What design parameters (process and measurement noise variance) give the best results?
Part 3: Play with the initial angular velocity of the alien artifact. What initial velocity vectors make it tumble maximally? What initial velocity vectors result in rotation about a fixed axis in space?
Part 4: Now control the lander (see controller.c for an example) to "land" on the alien artifact. Since we don't want to actually hit the alien artifact, this means you should continually thrust so as to fly just touching the tumbling alien artifact.
You can use any type of computer/OS/language you want. You can work in groups or alone.
Question: I was looking at the math library for quaternions that you gave us in the Lander package for Assignment 4... I think there may be an error in the compose_q( ) function in matrix3.c at line 148. According to Wolfram MathWorld it should be: result[0] = q[0]*p[0] - q[1]*p[1] - q[2]*p[*2*] - q[3]*p[3];
Answer: Yes, I agree. This is fixed in the lander02.zip release.
Question: animate.vcproj is missing from lander02.zip
Answer: Fixed in lander03.zip
Some general comments. It is usual for nonlinear systems to have the dynamics depend on the state: x_k+1 = F(x_k)*x_k + G(x_k)*u_k For an extended Kalman filter, you need to linearize F(x_k) at each x_k to produce F_k. http://en.wikipedia.org/wiki/Extended_Kalman_filter You should google "Kalman filter rotation orientation quaternions" to get examples of different approaches to doing this. More info http://en.wikipedia.org/wiki/Kalman_filter > Essentially the Kalman filter requires that we have an F such that, > for the state vector X, X_k = F*X_k-1. As I understand it F can > change, so that's actually F_k. I have selected the state vector that > is a 7-vector representing angular pose and angular velocity, the > first 4 being the quaternion, and the last three being the omegas > around world-coordinate axes. Now, your code demonstrates how to find > the derivative of omega and of the quaternion, but those functions are > not linear in X. yes. There are other state vectors you could use to represent orientation, such as a 3 vector representing the incremental orientation from the last orientation represented with a quaternion. > > The approach I've been starting on is to express them as matrix > equations of the form M*X, which could then be taken together to form > an F, but the problem is that the M ends up involving the state vector > explicitly. This is normal. > > My questions: > Is this a reasonable and correct state vector to use? reasonable, yes. There are many "correct" approaches. > Is it a problem to have F depend on X? That is the way it is supposed to be. > My suspicion is that the Kalman filter is best posed in world > coordinates, and that the moment of inertia of the alien artifact will > need to be transformed into that coordinate system at each time step. > Is this correct? Don't know if the Kalman filter is best posed in world coordinates. You can certainly pose it in world coordinates, or in body coordinates.
> For the markers placement on the alien aircraft. Can we assume that markers > are placed on the end points of the 3D polygon (rectangle) that you showed > us in class? No > I was wondering this because we were going to use this > assumption to state how you calculate the pose and angular velocity of the > alien aircraft. If we can assume this, then we can calculate the center of > the markers as center of the alien aircraft and calculate pose and angular > velocity from the center point/axis. Is this allowed? You can always define a center of the markers and use that as the "location" of an object. You can define pose and angular velocity wrt the markers, wherever they are, as long as you can register markers. Think about how the mocap system does this, wherever you place the markers on an object. Note that the center of the markers is not the center of mass of the object.
Question: I am totally confused about 3D Kalman filtering.
Answer: It is useful to do a 1D example first. Consider a bunch of markers on a record spinning in a record player (a fixed axis). The discrete time process is very simple (everything below is a scalar):
angle_k+1 = angle_k + T*angular_velocity_k angular_velocity_k+1 = angular_velocity_k + T*angular_acceleration_kPredicting the location of marker i is given by
x_prediction_i = x_i*cos(angle) - y_i*sin(angle) y_prediction_i = x_i*sin(angle) + y_i*cos(angle)Try designing the Kalman filter to track the angle of the record.
> We are having issues with dealing with creating the A matrix in the Kalman > Filter calculation. Let us have x_k be the state vector and x_k+1 be the > state vector that we are predicting, i.e. the equation > > x_k+1 = Ax_k + noise. > > We got rid of the B matrix because there is no input. > > We know our x vector = [q1 q2 q3 q4 wx wy wz], where q1,q2,q3,q4 are our > orientation of body coordinate for the alien aircraft and wx,wy,wz are > angular velocity of the alien aircraft. > > We have issues developing A matrix. We know that to get q's for the next > step (k+1), then it is q + q_dot, where q_dot is the change of each > component of our q. The formulas for this q relies on w so we had > > A = > > [ 1 -Wx/2 -Wy/2 -Wz/2 0 0 0 > Wx/2 1 -Wz/2 Wy/2 0 0 0 > Wy/2 Wz/2 1 -Wx/2 0 0 0 > Wz/2 -Wy/2 Wx/2 1 0 0 0 > ? ? ? ? ? ? ? > ? ? ? ? ? ? ? > ? ? ? ? ? ? ?] > > So our questions are as follows > > 1. Can we have the current w's in our matrix as well as x state vector as > shown above? Yes. > 2. What do we put in the bottom 3 rows to calculate the next state's w's? > Should we be using our estimated moment of Inertia that we have > calculated? Make use of 0 = I w_dot + w x I w