This assignment explores Kalman filtering with 3D kinematics and dynamics. It follows up on the previous assignment of tracking an alien spacecraft. In each part we will provide data files optained from sightings of the alien artifact (noisy marker data files), and you will provide us with estimates of the state of the alien artifact. The learning objectives are to learn about how to use the Kalman Filter to process noisy data and generate missing measurements, to use an Extended Kalman Filter, and to learn about SLAM.
In this part we will give you the answer, to help you and clarify what we are after. Do not turn anything in for Part 0.
In this part the noisy marker file is markersn in part0.zip. For this part the artifact was in stealth mode, which makes it transparent, and all markers are in view at all times. Furthermore, the artifact seems to have time invariant dynamics, and is not under power. Implement a Kalman filter to estimate center of mass location and COM velocity at each sample time. We recommend using Matlab to do this.
The files we give you are noisy marker files. In this part we give
you one file "markersn".
Each line of a marker file has x, y, z locations of 8 markers.
m0x m0y m0z m1x m1y m1z ... m7x m7y m7z
The sampling rate is 10Hz.
The marker locations on the body (in body coordinates relative to COM) are:
Marker X Y Z 0 -1.5 -3.0 -4.5 1 -1.5 -3.0 1.5 2 -1.5 1.0 -4.5 3 -1.5 1.0 1.5 4 0.5 -3.0 -4.5 5 0.5 -3.0 1.5 6 0.5 1.0 -4.5 7 0.5 1.0 1.5
This configuration of markers is the "zero" orientation (quaternion q = (1 0 0 0)), and other rotational conventions (what the quaternion means) are as in assignment 2. In part 0 the orientation of the artifact is constant at q = (1 0 0 0).
You will need to create a process model including an estimate of process noise, and a measurement model including an estimate of measurement noise. The initial position and velocity are chosen randomly, and you will have to estimate their initial variance.
Hint: Use your assignment 2 code to generate noise free data and debug your filter on that. Then add noise to the markers. Debug your filter on that. See controller.c in the answer zip file. Generating your own test data is a good idea on each part of the assignment.
The code to generate the data (*.c, *.h; see README.TXT) and the code to Kalman filter the data (*.m) are in part0.zip.
Note: This problem is similar to visually tracking rigid objects with no occlusion.
The 10 noisy marker files we give you part1.zip are similar to those in Part 0 (no occlusion, no power), but the artifact starts in an unknown orientation and is rotating as well as translating. Implement a Kalman filter to estimate center of mass location, COM velocity, artifact orientation, and artifact angular velocity at each sample time. We recommend modifying the Part 0 Matlab code to do this.
For each file, you should create a corresponding file containing state
estimates:
x y z xd yd zd q0 q1 q2 q3 wx wy wz
The file should have names p1a00, p1a01, .... (p1 = part1, a = answer).
Separately, you should provide a writeup of how you created the Kalman
Filter, how you generated process, measurement, and initial state models,
and how the filter works.
Again, you will need to create a process model including an estimate of process noise, and a measurement model including an estimate of measurement noise. You can use these inertial parameters (the same as what was used in assignment 2) (I_body is diagonal):
I_body_x = 1.571428571428571; I_body_y = 5.362637362637362; I_body_z = 7.065934065934067;The initial state is chosen with random positions, orientations, velocities, and angular velocities.
Hint: You will need to use an extended Kalman filter to handle the quaternion part. Google "quaternion Kalman filter" or "Kalman filter rotation orientation quaternion" for more information, and papers on several different ways to do this.
The Extended Kalman Filter linearizes the dynamics and measurement equations about a nominal state. It is useful to represent the nominal orientation with a quaternion, but you can represent the deviation from the nominal state using XYZ world fixed Euler angles (ax, ay, az). For small angles this representation is linear in the angles, and makes everything much simpler. The rotation matrix for Euler XYZ representation is:
cy*cz -cy*sz sy cx*sz+cz*sx*sy cx*cz-sx*sy*sz -cy*sx sx*sz-cx*cz*sy cx*sy*sz+cz*sx cx*cywhich becomes the following when cos(angle) = 1 and sin(angle) = angle.
1 -az ay az 1 -ax -ay ax 1These deviations are incorporated into the nominal orientation (the quaternion) after each process and measurement update, and the state variables ax, ay, and az zeroed out. This can be done in relinearize.m.
Hint: Use your assignment 2 code to generate noise free data and debug your filter on that. Then add noise to the markers.
Hint: on the first try, ignore the effect of angular velocity and orientation on the rotational dynamics. Let the angular acceleration just be generated by the process noise.
for i = 1:3 % position = position + T*velocity state_estimate(i) = state_estimate(i) + T*state_estimate(i+3); % angle = angle + T*angular_velocity state_estimate(i+6) = state_estimate(i+6) + T*state_estimate(i+9); endThe estimated angular velocities should lag the real angular velocities. To fix this take into account the effect of angular velocity on angular acceleration (assuming Iworld is fixed):
wd = -inv(Iworld)*[wx]*Iworld*wThe angular velocities should no longer lag the real angular velocities. Then take into account the effect of orientation on Iworld. This allows the filter to know that orientation estimation errors lead to errors in predicting future states.
Iworld = Ra*Rq*Ibody*Rq'*Ra'Rq is the nominal rotation due to the nominal orientation given by the quaternion q. Ra is the rotation due to the Euler angle offsets (ax, ay, az). Note that depending on how you define Rx, the transposes could go on the other side.
Hint: use Mathematica or an equivalent symbolic dynamics program to derive formulas you can then linearize. Let's do
wd = -inv(Iworld)*[wx]*Iworld*wfirst.
Iworld = {{ixx,ixy,ixz},{ixy,iyy,iyz},{ixz,iyz,izz}}
w = {{wx},{wy},{wz}}
wcross = {{0,-wz,wy},{wz,0,-wx},{-wy,wx,0}}
wd = -Expand[Simplify[Inverse[Iworld].(wcross.(Iworld.w))]]
(* pull out determinant to simplify things. *)
detIworld = Det[Iworld]
wdnodet = Expand[Simplify[Expand[wd*detIworld]]]
(* take partial derivative dwdx/dwx which is a part of the appropriate *)
(* A matrix entry (need a dwx(k+1)/dwx(k) = 1 + T*dwdx/dwx ) *)
CForm[D[wdnodet[[1]],wx]/detIworld]
(* now do rest of dwdi/dwj *)
The effect of orientation is really messy due to the Inverse[Iworld] and ax, ay, and az show up in the denominator. It would be reasonable to skip this part, but here it is
Ibody = {{ixx,ixy,ixz},{ixy,iyy,iyz},{ixz,iyz,izz}}
Rq = {{rxx,rxy,rxz},{ryx,ryy,ryz},{rzx,rzy,rzz}}
Ra = {{1,-az,ay},{az,1,-ax},{-ay,ax,1}}
Iworld = Ra.Rq.Ibody.Transpose[Rq].Transpose[Ra]
% need to check on where Transpose[] actually goes above.
w = {{wx},{wy},{wz}}
wcross = {{0,-wz,wy},{wz,0,-wx},{-wy,wx,0}}
wd = -Expand[Simplify[Inverse[Iworld].(wcross.(Iworld.w))]]
After taking derivatives, any terms that include ax, ay, or az
are zero since the nominal value for these offsets is zero.
Add terms dwi(k+1)/daj to the A matrix.
This problem is similar to visually tracking rigid objects with occlusion.
Part 2: Redo part 1 with these files part2.zip that are similar to part 1, except occluded markers have values 1e10 1e10 1e10 for x, y, and z. One way to handle this is to switch measurement models for each combination of occluded markers. Turn in files corresponding to the data files we give you in the part 1 tracking file format, and with names p2a00, p2a01, .... Separately, you should provide a writeup of what you changed for this part. To make this simpler we only occlude up to 1 marker from each observation. The occluded marker is the furthest from the lander (not quite right, but good enough for this assigment).
This an instance of a SLAM problem, and the structure from motion problem in vision.
Part 3: Redo part 2 with these files part3.zip that are similar to part 1 and 2,
except the markers locations are not quite the same as in part 1 and 2.
You need to estimate corrected marker locations as well. The marker locations
are fixed in body coordinates. We are back to the no occlusion case.
Turn in files in the part 1 tracking file format with marker locations
appended at the end of each line:
x y z xd yd zd q0 q1 q2 q3 wx wy wz m0x m0y m0z m1x m1y m1z ... m7x m7y m7z
and with names p3a00, p3a01, ....
Separately, you should provide a writeup of what you changed for this
part.
This part is for extra credit.
Bonus: You used an
EMP
weapon to disable the artifact. It is now tumbling passively and bits
are falling off.
The dynamic model is changing. Track the marker locations (as in part 3)
and the moment of inertia matrix.
Redo part 3 with these files part4.zip
Turn in files in the part 3 tracking file format with a moment of inertia
matrix
appended at the end of each line:
x y z xd yd zd q0 q1 q2 q3 wx wy wz m0x m0y m0z m1x m1y m1z ... m7x m7y m7z Ixx Ixy Ixz Iyy Iyz Izz
and with names p4a00, p4a01, ....
Separately, you should provide a writeup of what you changed for this
part.
You can use any type of computer/OS/language you want. You can work in groups or alone.
Question: I am totally confused about 3D Kalman filtering.
Answer:
There is a lot of info on the Kalman filter on the web:
Wikipedia,
Welch,
or just Google "Kalman filter", ...
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_k
Predicting 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 not quite sure about the difference between transient and steady
> version of Kalman filter? Could you please enlighten us? Thank you!
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.
Can we decouple the filter into one for translation and one for rotation?
Even though linear and rotational dynamics are decoupled, since the
same markers are used we get more filtering power if we combine the
dynamics. The measurement errors have to be the same for both translation
and rotation, so processing the dynamics together allows the filter to
take advantage of this.
Can we numerically differentiate predict_markers() to get C and
do_dynamics() to get A, instead of wallowing through all that
Mathematica stuff?
Yes. You can also use numerical differentiation to check your symbolic
A and C matrices. Here is some Matlab code to do this.
% check C (measurement derivatives)
% handles ax, ay, az by relinearizing
% Nominal values of ax, ay, and az should be zero!!!!
q = [ 1 0 0 0 ]';
for i = 1:4
q(i) = rand;
end
q_norm = q(1)*q(1) + q(2)*q(2) + q(3)*q(3) + q(4)*q(4);
q = q/sqrt(q_norm);
q'
s = [ 0 0 0 0 0 0 0 0 0 0 0 0 ]';
for i = 1:6
s(i) = rand;
end
for i = 10:12
s(i) = rand;
end
s'
index = 10;
ss = s;
[ qqq, sss ] = relinearize( q, ss );
[ me, C ] = predict_markers( qqq, sss, index, markersn );
CC = zeros( 3*n_m, n_x );
d = 1e-4;
for i = 1:n_x
ss = s;
ss(i) = s(i) - d;
[ qqq, sss ] = relinearize( q, ss );
me_m = predict_markers( qqq, sss, index, markersn );
ss = s;
ss(i) = s(i) + d;
[ qqq, sss ] = relinearize( q, ss );
me_p = predict_markers( qqq, sss, index, markersn );
for j = 1:(3*n_m)
CC(j,i) = (me_p(j) - me_m(j))/(2*d);
end
end
CC - C
% check A (process derivatives)
% doesn't handle ax, ay, az correctly
% just need to put in relinearize()?
q = [ 1 0 0 0 ]';
for i = 1:4
q(i) = rand;
end
q_norm = q(1)*q(1) + q(2)*q(2) + q(3)*q(3) + q(4)*q(4);
q = q/sqrt(q_norm);
q'
s = [ 0 0 0 0 0 0 0 0 0 0 0 0 ]';
for i = 1:6
s(i) = rand;
end
for i = 10:12
s(i) = rand;
end
s'
[ s_next, A ] = do_dynamics( q, s );
AA = zeros( n_x, n_x );
d = 1e-2;
for i = 1:n_x
ss = s;
ss(i) = s(i) - d;
sm = do_dynamics( q, ss );
ss = s;
ss(i) = s(i) + d;
sp = do_dynamics( q, ss );
for j = 1:n_x
AA(j,i) = (sp(j) - sm(j))/(2*d);
end
end
AA - A