Assignment 2

by Jon Boerner & Andrew Burks
Download C Code
Download MatLab Code

Part 1: Playing Around


We had a lot of fun playing around with the angular velocities. Combining angular velocity in the x, y, and z direction seemed to produce maximum tumble for us.

Part 2: Estimate Future Trajectory

2a: find center of mass and orientation as a function of time


DATA FILE: problem_2_0.dat

We knew the key to the center of mass and orientation was the homogeneous transformation matrix that turned some point in "Artifact Coordinates" into a point in "Lander Coordinates" AKA "Assignment Coordinates". 8 Points of data is actually way more than you need. While you can find the transformation matrix with just three points, we used four for simplicity.

We knew that if you took one vertex of the artifact and called that the "origin" of the new artifact coordinate system, you could then take the three points that border the "origin" point and subtract the "origin" point vector from each of those 3 points' vectors to yield three orthogonal vectors that can define a new artifact coordinate system. Normalizing each of those orthogonal vectors produced unit vectors in the new coordinate system.

We chose to base our coordinate system around point 4. This choice was made so that in the initial configuration the axes of the artifact system aligned with the axes of the assignment system. Projecting the new "x" unit vector onto the "x" axis of the assignment coordinate system produces the first column of the rotation matrix that we are looking for. Projecting the new "y" and "z" unit vectors produced the next two columns.

The vector describing point 4 in assignment coordinates becomes the fourth column in the homogeneous transformation matrix. Of course, the bottom row of the Homogenous transformation matrix is, as always, [0 0 0 1]. At this point, we are able to determine the location and orientation of any given point within the artifact in assignment coordinates.

The center of mass of an object is not affected by rotation. Because there are no forces or torques acting on the artifact, the center of mass moves at a constant velocity in space. Our algorithm for finding the center of mass exploited this fact.

While optimizing for the center of mass, we needed a method for scoring some guess as a location of the center of mass (in artifact coordinates). The first thing we did was use the series of transformation matrices we had obtained earlier to find the position of the guess at every point in time in the assignment coordinates. Next, we ran a linear regression of the x, y, and z coordinate of the point independently. We then summed the squares of the resiguals to obtain a score. As long as there is some angular velocity, the center of mass would have a score of zero, and all other points would have a positive score.

Each iteration of our optimization involved looking at the 8 corners of a rectangle similar to the artifact, but which became twice as small in each dimension every iteration. The center of the guess rectangle was the previous iteration's "winner" (initially the center of the artifact). Originally, the length of each side of the rectangular prism was half the total length of its corresponding side on the artifact. This means that each of the 8 points that were scored in each iteration would be at the center position of a smaller guess rectangle if they were the "winner". This optimization technique was very quick because it decreased the search space 8-fold each iteration, although it would be susceptible to local minima. In just 17 iterations (less than a second on the computer used) the center of mass was determined to at least .00001 precision in each dimension.

This MatLab function read in data from an excel sheet where each of the 8 points was in its own sheet, then produced and returned the rotation matrix and transformation matrix:
function [ Hla , Rla] = readData( filename)
[P0,~,~] = xlsread(filename,'P0');
[P1,~,~] = xlsread(filename,'P1');
[P2,~,~] = xlsread(filename,'P2');
[P4,~,~] = xlsread(filename,'P4');

xh = ((P4-P0)./2)';
yh = ((P2-P0)./4)';
zh = ((P1-P0)./6)';
dh = P0';

Rla(:,1,:) = xh(:,:);
Rla(:,2,:) = yh(:,:);
Rla(:,3,:) = zh(:,:);

Hla = Rla;
Hla(:,4,:) = dh(:,:);
Hbotrow = zeros(1,4,size(Hla,3));
Hbotrow(1,4,:) = ones(1,1,size(Hla,3));
Hla(4,:,:) = Hbotrow;
end
This MatLab function ranked a guess point in assignment coordinated as a potential center of mass:
function [ score ] = rankCOM( p )
t = 1:size(p,3);
T = zeros(1,1,size(p,3));
T(1,1,:) = t(:);

xd = polyfit(T,p(1,1,:),1);
Xproj = xd(1).*T(1,1,:) + xd(2);
Xr2 = (Xproj(1,1,:) - p(1,1,:)).^2;
Xr2sum = sum(Xr2);

yd = polyfit(T,p(2,1,:),1);
Yproj = yd(1).*T(1,1,:) + yd(2);
Yr2 = (Yproj(1,1,:) - p(2,1,:)).^2;
Yr2sum = sum(Yr2);

zd = polyfit(T,p(3,1,:),1);
Zproj = zd(1).*T(1,1,:) + zd(2);
Zr2 = (Zproj(1,1,:) - p(3,1,:)).^2;
Zr2sum = sum(Zr2);

score = Xr2sum + Yr2sum + Zr2sum;
end
This MatLab function ran the optimization to find the center of mass:
function [ center] = findCOM( Hla, criteria )
BOX = [2 ; 4 ; 6];
center = [1 ; 2 ; 3];
makeCorners = [-1 -1 1 1 -1 -1 1 1 ; 1 1 1 1 -1 -1 -1 -1 ; -1 1 -1 1 -1 1 -1 1];
scales = ones(1,3);
corners = zeros(3,8);
scores = ones(1,8);

i = 1;
while max(scales) > criteria
    scale = 2^(i-1);
    scales = (BOX/4)/scale;
    for j=1:8
        corners(:,j) = center + makeCorners(:,j).*scales;
        scores(1,j) = rankCOM(homoTransform(Hla,[corners(:,j);1]));
    end
    [~, index] = min(scores);
    center = corners(:,index);
    i = i+1;
end
end
It was neccessary to create a helper function to turn a series of artifact points into a series of assignment points: 
function [ pa ] = homoTransform( Hab , pb )
pa = zeros(4,1,size(Hab,3));
for i=1:size(Hab,3)
    pa(:,:,i) = Hab(:,:,i)*pb;
end
end
It was also neccessary to create a function to convert the rotation amtrices used into quaternions for grading: 
function [ q ] = RtoQ( R )
T = 1 + R(1,1) +R(2,2) + R(3,3);
if(T > .0000001)
    S = 2*sqrt(T);
    q(1) = .25*S;
    q(2) = (R(2,3)-R(3,2))/S;
    q(3) = (R(3,1)-R(1,3))/S;
    q(4) = (R(1,2)-R(2,1))/S;
elseif(R(1,1) >= R(2,2) && R(1,1) >= R(3,3))
    S = (1 + R(1,1) - R(2,2) - R(3,3))^.5;
    q(1) = (R(2,3) - R(3,2))/S;
    q(2) = .25*S;
    q(3) = (R(1,2) + R(2,1))/S;
    q(4) = (R(3,1) + R(1,3))/S;
elseif(R(2,2) > R(3,3))
    S = (1 + R(2,2) - R(1,1) - R(3,3))^.5;
    q(1) = (R(3,1) - R(1,3))/S;
    q(2) = (R(1,2) + R(2,1))/S;
    q(3) = .25*S;
    q(4) = (R(2,3) + R(3,2))/S;
else
    S = (1 + R(3,3) - R(1,1) - R(2,2))^.5;
    q(1) = (R(1,2) - R(2,1))/S;
    q(2) = (R(3,1) + R(1,3))/S;
    q(3) = (R(2,3) + R(3,2))/S;
    q(4) = .25*S;
end
end

2b: find angular velocity as a function of time


DATA FILE: problem_2_1.dat

The equation we used for angular velocity is W = Rdot*R', Where Rdot is the change in R between two consecutive frames, divided by the timestep between those frames. The W obtained is a skew symetric matrix. This was verified because the main diagonal of this matrix was very close to zero. wx, wy, and wz, which comprize the angular velocity, "w", can be obtained from the angular velocity tensor from this method.

This matlab function produced the angular velocity (and acceleration) from a given set of rotation matrices: 
function [ w , wd] = getw( Rla,timestep)
Rlad = zeros(3,3,size(Rla,3)-1);
w = zeros(3,1,size(Rla,3)-1);
wd = zeros(3,1,size(Rla,3)-2);
for i=1:(size(Rla,3)-1)
    Rlad(:,:,i) = (Rla(:,:,i+1)-Rla(:,:,i))/timestep*(Rla(:,:,i)');
    w(1,1,i) = Rlad(3,2,i);
    w(2,1,i) = Rlad(1,3,i);
    w(3,1,i) = Rlad(2,1,i);
    if(i > 1)
        wd(:,:,i-1) = (w(:,:,i)-w(:,:,i-1))/timestep;
    end
end
end

2c: find angular acceleration as a function of time


DATA FILE: problem_2_2.dat

The angular acceleration was fairly easy to find once we had the angular velocities for each moment in time. By taking the difference between the angular velocities of two consecutive points in time and dividing by the timestep we produced the angular acceleration vector.

The same piece of MatLab code (above) produced the angular acceleration at the same time as the angular velocity, for convenience.

2d: find the moment of inertia and principal axes of inertia

    .1732   .0023   .0006
I = .0023   .5968  -.0001
    .0006  -.0001   .7834
Principal moments are .1732, .5968, and .7834. These are a scalar multiple of the given moments of inertial. The constant multiplier is .1108. In the given initial position, the rotation (in quaternions) is [1 ; 0 ; 0 ; 0], because the start aligned.

To calculate the inertia of the artifact, I first needed to convert my angular velocities and accelerations into body coordinates so that the Inertia would be constant. Using the equation A*I6=T given on the website and Singular Value Decomposition I was able to obtain the 6x1 Inerita matrix. The 6x1 Inertial matrix was simply the sixth and final column in the resulting 'V' matrix.

Composing the neccessary A matrix was a bit of a challenge. A MatLab function was made to create an A matrix composed from a certain number of data points at specific intervals. The final A used contained 500 data points space .1 seconds apart (for the first 50 seconds). Because the off-diagonal elements of the reconstructed Inertia Matrix were nearly zero, this coordinate system was assumed to be aligned with the principal axes. However, if this had not been the case, we would have optimized across several guess rotations to find the rotation of I that creates the smallest off-diagonal elements.

This MatLab function was used to construct the A matrix: 
function [ A ] = buildA( wB, wdB, qty , spacing )
    if (spacing*(qty-1)) >= size(wdB,3)
        error('NOT ENOUGH DATA POINTS\n');
    end
    A = zeros(3*qty,6);
    for i=1:qty
        w = wB(:,1,1+((i-1)*spacing));
        wd = wdB(:,1,1+((i-1)*spacing));
        Awd = [wd(1) 0 0 0 wd(3) wd(2) ; 0 wd(2) 0 wd(3) 0 wd(1) ; 0 0 wd(3) wd(2) wd(1) 0];
        Aw = [w(1) 0 0 0 w(3) w(2) ; 0 w(2) 0 w(3) 0 w(1) ; 0 0 w(3) w(2) w(1) 0];
        Tw = [0 -w(3) w(2) ; w(3) 0 -w(1) ; -w(2) w(1) 0];
        Ai = Awd + (Tw*Aw);
        A(1+((i-1)*3),:) = Ai(1,:);
        A(2+((i-1)*3),:) = Ai(2,:);
        A(3+((i-1)*3),:) = Ai(3,:);
    end
end
This MatLab function was used to find the 3x3 Inertial Matrix using the built in SVD function: 
function [ I9 ] = findI9( Rla, w, wd )
wB = zeros(3,1,size(w,3));
wdB = zeros(3,1,size(wd,3));
for i=1:size(wd,3)
    wB(:,:,i) = Rla(:,:,i)' * w(:,:,i);
    wdB(:,:,i) = Rla(:,:,i)' * wd(:,:,i);
end
i = size(w,3);
wB(:,:,i) = Rla(:,:,i)' * w(:,:,i);

A = buildA(wB,wdB,500,10);
[U,S,V] = svd(A);
I6 = V(:,6);

I9 = makeI9(I6);
I9 = Rla(:,:,1)*I9;
end
The 6x1 Inertia Matrix used in calculations was converted to a 3x3 matrix with this function: 
function [ I9 ] = makeI9( I6 )
I9 = [I6(1) I6(6) I6(5) ; I6(6) I6(2) I6(4) ; I6(5) I6(4) I6(3)];
end

2e: predicting the future trajectory of the object


DATA FILE: problem_2_3.dat

Forward dynamics is just the opposite of what I've been doing the entire time. Instead of finding Inertia based on w and wd, I find wd based on Inertia and w, etc. Here is the process I used at each step in time to determine all of the neccessary variables for the next step: The only inputs required to predict forward are the Inertia Tensor, Center of Mass in assignment coordinates, Initial rotation, initial angular velocity, and the timestep.

This matlab function handled all of the forward dynamics: 
function [ RlaP , COMlP] = forwardDynamics( HlaS,RlaS,wS,COM,COMd,I9,start,stop,timestep)
%new coordinate system at COM
wP = zeros(3,1,stop-start+1);
wdP = zeros(3,1,stop-start+1);
RlaP = zeros(3,3,stop-start+1);
COMlP = zeros(4,1,stop-start+1);

wP(:,:,1) = RlaS'*wS;
RlaP(:,:,1) = RlaS;
COMlP(:,:,1) = HlaS*[COM ; 1];
wdP(:,:,1) = I9\(-1*cross(wP(:,:,1),I9*wP(:,:,1)));

for i=1:(stop-start)
    wP(:,:,i+1) = wP(:,:,i) + wdP(:,:,i)*timestep;
    wdP(:,:,i+1) = I9\(-1*cross(wP(:,:,i+1),I9*wP(:,:,i+1)));
    Rlad = [0 -wP(3,1,i) wP(2,1,i) ; wP(3,1,i) 0 -wP(1,1,i) ; -wP(2,1,i) wP(1,1,i) 0];
    RlaP(:,:,i+1) = RlaP(:,:,i) + timestep*(Rlad*RlaP(:,:,i));%minus! 
    COMlP(:,:,i+1) = COMlP(:,:,i) + timestep*[COMd ; 0];
end
end
Unfortunately, there was some considerable drift observed. By the end of the 10 seconds, the rotations were about 15% off what they should be.

Part 3: Control The Lander


DATA FILE: problem_3.dat

Orientation Control

For controlling the orientation of the lander, we used the provided PD control and only had to worry about setting the correct desired quaternion. We did this by first finding the rotation matrix of the artifact with respect to the world, and then converting that rotation matrix to a quaternion. Our inital implementation for this, however, had discontinuities in the quaternions. In brief, the scalar term never became negative, causing jumps in the non-scalar terms. We used the previous two desired-orientation-quaternions to calculate a predicted desired-orientation-quaternion for the current timestep using linear interpolation, and just determined whether the calculated quaternion or its negative was the logical choice for the current timestep. The code used for this was: 

void r2q(double r[3][3], double* q, SIM* s){
  double T = 1 + r[0][0] + r[1][1] + r[2][2];
  if(T > .0000001){
    double S = sqrt(T) * 2;
    q[0] = .25*S;
    q[1] = (r[1][2] - r[2][1])/S;
    q[2] = (r[2][0] - r[0][2])/S;
    q[3] = (r[0][1] - r[1][0])/S;
  }else if(r[0][0] > r[1][1] && r[0][0] > r[2][2]){
    double S = sqrt(1 + r[0][0] - r[1][1] - r[2][2]);
    q[0] = (r[1][2] - r[2][1])/S;
    q[1] = .25*S;
    q[2] = (r[0][1] + r[1][0])/S;
    q[3] = (r[2][0] + r[0][2])/S;
  }else if(r[1][1] > r[2][2]){
    double S = sqrt(1 - r[0][0] + r[1][1] - r[2][2]);
    q[0] = (r[2][0] - r[0][2])/S;
    q[1] = (r[0][1] + r[1][0])/S;
    q[2] = .25*S;
    q[3] = (r[1][2] + r[2][1])/S;
  }else{
    double S = sqrt(1 - r[0][0] - r[1][1] + r[2][2]);
    q[0] = (r[0][1] - r[1][0])/S;
    q[1] = (r[2][0] + r[0][2])/S;
    q[2] = (r[1][2] + r[2][1])/S;
    q[3] = .25*S;
  }
  
  double lq2[4];
  vector_cpy(lq2, s->a_lq,4);
  multiplize(lq2,2,4);//scalar multiplication by 2
  double p[4], nq[4], ep[4], en[4];
  vector_sub(lq2,s->a_llq,p,4);//subtraction
  vector_cpy(nq,q,4);
  multiplize(nq,-1,4);//scalar multiplication by -1
  vector_sub(q,p,ep,4);
  vector_sub(nq,p,en,4);
  if(norm4(en) < norm4(ep)){
    vector_cpy(q,nq,4);
  }
}
where the previous quaternions are stored in s->a_lq and s->a_llq.

The first rotation matrix (artifact with respect to the lander) for the current timestep was found using normalized vectors pointing from corner 0 to corners 1, 2, and 4. These three unit vectors formed a basis for coordinates in an artifact frame, and can thus be used to directly construct a rotation matrix by using the unit vectors as columns. The second rotation matrix (the lander with respect to the world) was provided. These two rotation matrices were multiplied to find the rotation matrix of the artifact with respect to the world.

The quaternion described above created from the rotation matrix described above was used as the desired orientaion for the PD control. The P gain was 100 and the D gain/loss was 4.

An interesting side note is that we had mistakenly used the artifact's rotation matrix with respect to the world supplied instead of the lander's matrix for the same thing. Funnily enough, the code still worked since the initial orientation of the artifact and lander were the same and the PD control guaranteed they stayed that way as time went by.

Position Control

After increasing the gains in the provided PD control, this problem became just a matter of selecting some key waypoints for the lander to follow. The first waypoint was location well above the face of the artifact that it was planning on landing on, and was constantly changing. This initial point would keep the lander from crashing into the artifact when it moved from its initial position. The second and final waypoint was a position just barely above the landing face.

Each of these waypoints is static in artifact coordinates. Because at this point we knew the location of the center of mass from our previous analysis, we could identify the x,y, and z positions of the center of mass in artifact coordinates. The desired y and z coordinates in both waypoints were identical to the y and z coordinates of the center of mass. Only the x position changed. It was initially quite large to avoid collisions, but in the final waypoint there was a distance that accounted for the size of the lander and distance from the COM of the artifact to its surface.

There are 3 important vectors that need to be added together to get a point in world coordinates that we can control towards:

Landing Video