This assignment explores Kalman filtering, and 3D orientation, dynamics, and 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.
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
> In the homework assignment 4, can we say we know the geometry of the alien > artifact(length and the the positons of the markers wrt alien artifact)? > Or we don't know anything except the noisy positions of 8 markers? You don't know anything except the noisy positions of 8 markers. You need to identify the marker locations and the center of mass location. Chris
> Hi, > > For assignment 4 of KDC: > > From my understanding, we need to create the model of the measurement > zk=h(xk) for the use of Kalman filter. But we are using this only for the > calculation of Kalman filter. We are not using this to "measure" the > locations of the markers? Is this correct? > > And, the measurement vertor zk would contain all the x/y/z coordinates of > 8 markers? and function h(xk) transform our state vector > (wx,wy,wz,q0,q1,q2,q3) into the coordinates of 8 markers? > > In the assignment statement, it also mentioned that we need to estimate > the moment of inertia matirx for the alien. How should we specify the > distribution of the mass? Uniformly distributing at 8 markers? > > Thanks! In dynamics.c I provide the measurements at the part of the code marked // create markers // THIS IS THE MEASUREMENT PROCESS AND THE CONVERSION TO LANDER COORDINATES The noisy data ends up in s->markers_noisy[i][j] and is xyz vectors. You need to duplicate this measurement process (except the adding noise part) for the design of your Kalman filter (and also take the derivative of it). You need to estimate the moment of inertia (or ratio of the principal moments and their orientation from movement data, since you have no way to know the density of the alien spacecraft. So: > We are not using this to "measure" the > locations of the markers? Is this correct? yes. My code provides the noisy measurements. > And, the measurement vertor zk would contain all the x/y/z > coordinate of the 8 markers? yes > function h(xk) transform our state vector > (wx,wy,wz,q0,q1,q2,q3) into the coordinates of 8 markers? yes > How should we specify the > distribution of the mass? Uniformly distributing at 8 markers? no. You need to observe motion and identify the location of the center of mass and moment of inertia.
> We are not quite sure about the difference between transient and steady > version of Kalman filter? Could you please enlighten us? Thank you! There is a lot of info on the Kalman filter on the web n.wikipedia.org/wiki/Kalman_filter www.cs.unc.edu/~welch/kalman/ ... Basically, when A, B, Q, and R are constant, the Kalman gain K and the estimation error covariance P will converge to constants (steady state values). That is the "steady state" filter. Everything else is transient. Check out http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf especially page 6.