We did some more research for inertial measurement units. We found four more companies and ed them about their product information.

Transcription

1 We did some more research for iertial measuremet uits. We foud four more compaies ad ed them aout their product iformatio. BAE Systems MicroSCIRAS KVH Summit Istrumets Their data sheets are posted i our oteooks. The IMU from Summit Istrumets is aout $25,000 ecause they deal maily with the military, so they are out of our price rage. The relevat drift iformatio for these compaies are posted o the iterestig products lik o our wesite. We made a ew fuctio called earth_param, that has all the earth costat parameters listed. This fuctio will retur a array as show i the code elow: fuctio [earth]=earth_param() EARTH_PARAM Returs the costat parameters of the earth for use i differet fuctios. [earth] = earth_param() Returs: a - earth equitorial radius - earth polor radius e - eccetricity of elipsoid f - (a-)/a o - omega - earth tur rate Writte By: Bria Bleeker Ro MacMilla earth.a = ; earth. = ; earth.f = (earth.a-earth.)/earth.a; earth.e = sqrt(earth.f*(2-earth.f)); earth.o = E-5; The we had to chage our origial fuctios, ecef2ed ad ed2ecef to accomidate this chage. The ew code is show elow: fuctio [NED] = ecef2ed(gtime, ecefpos) ECEF2NED Covert ecef coordiates to NED coordiates ad diplay. [NED] = ecef2ed(gtime, ECEF positio) ECEF Positio Array = [x, y, z] Example: array.x array.y array.z *** The array parameters must have *.x, *.y, ad *.z.

2 Eter the time, ad the x,y,z positios i ecef coordiates. This fuctio will display two figures: 1: x,y,z positio i ecef coordiates ad 2: latitude, logitude, ad altitude NED is retured with 3 paramerters: 1: NED.lat - latitude 2: NED.lo - logitude 3: NED.alt - altitude A egative logitude is the Wester hemispere. A egative latitude is i the Souther hemisphere. Writte By: Bria Bleeker Ro MacMilla earth = earth_param; parameters get earth gtime = gtime(:,1) - gtime(1,1); figure(1), suplot(311), plot(gtime, ecefpos.x), grid title('ecef X Positio') suplot(312), plot(gtime, ecefpos.y), grid title('ecef Y Positio') suplot(313), plot(gtime, ecefpos.z), grid title('ecef Z Positio') le = legth(ecefpos.x); data get legth of for i = 1:le lo(i) = ata2(ecefpos.y(i), ecefpos.x(i)); log = ata(y,x) - direct lo(i) = lo(i)*180/pi; covert to degrees h = 0; N = earth.a; flag = 0; j = 0; p = sqrt(ecefpos.x(i)^2 + ecefpos.y(i)^2); iitialize silat = ecefpos.z(i)/(n*(1-earth.e^2)+h); First iteratio lat(i) = ata((ecefpos.z(i)+earth.e^2*n*silat)/p); N = earth.a/(sqrt(1 - (earth.e^2)*(silat^2))); prevalt = (p/cos(lat(i)))-n; prevlat = lat(i)*180/pi; while (flag < 2) do at least 100 iteratios flag = 0; silat = ecefpos.z(i)/(n*(1-earth.e^2)+h); lat(i) = ata((ecefpos.z(i)+earth.e^2*n*silat)/p);

3 ed ed N = earth.a/(sqrt(1 - (earth.e^2)*(silat^2))); alt(i) = (p/cos(lat(i)))-n; lat(i) = lat(i)*180/pi; if as(prevalt-alt(i)) < flag = 1; ed if as(prevlat-lat(i)) < flag = flag + 1; ed j = j+1; if j == 100 flag = 2; ed prevalt = alt(i); prevlat = lat(i); NED.lat = lat; NED.lo = lo; NED.alt = alt; figure(2), suplot(311), plot(gtime, NED.lo), grid title('ned Logitude') suplot(312), plot(gtime, NED.lat), grid title('ned Latitude') suplot(313), plot(gtime, NED.alt), grid fuctio [ecef_pos] = ed2ecef(ed) NED2ECEF2 Coverts NED coordiates to ECEF coordiates. [ecef_pos] = ed2ecef(ed) Returs: ecef_pos.x - x positio ecef_pos.y - y positio ecef_pos.z - z positio Writte By: Bria Bleeker Ro MacMilla earth = earth_param; N = (earth.a)/(sqrt(1-(earth.e)^2*(si((ed.lat*(pi/180))))^2)); coslat = cos(ed.lat*(pi/180)); silat = si(ed.lat*(pi/180)); coslo = cos(ed.lo*(pi/180)); silo = si(ed.lo*(pi/180)); ecef_pos.x = (N+ed.alt)*coslat*coslo; ecef_pos.y = (N+ed.alt)*coslat*silo; compute the curret logitude ecef_pos.z = (N*(1-earth.e^2)+ ed.alt)*silat;

4 The we had a discussio with Dr. Ah. We showed him our lock diagram for the attitude computer. He gave us some suggestios o what we should chage. The ew lock diagram for the attitude computer ca e see i FIG : Earth Data R o L, altitude Get raw agular data from IMU ω i Compute: ω e ω ie Compute: [ ω ] ω + = ωi C e ωie Otai the iitial attitude agles Skew Matrix: Ω = skew[ ω ] Iitial: C (0) Update C : C [ C ( t) Ω ( )] ( t + t) = C ( t) + t t Updated Platform Agles { α, β γ } 0 0, 0 Fig - (Block Diagram for attitude computer)

5 Now we eed to create the rest of the fuctios to complete the lock diagram for the attitude computer. We wat to create a separate fuctio for each lock. First, we created a fuctio for the tur rate of the earth with respect to the iertial frame i the NED frame. fuctio [wi] = trearth_if_ned(lat) TREARTH_IF_LGF Fid the tur rate of the earth with respect to the iertial frame i the local NED frame. [wi] = trearth_if_ned(latitude) Latitude is i decimal degrees. Writte By: Bria Bleeker Ro MacMilla earth=earth_param; wi(1) = earth.o * cos(lat*pi/180); wi(2) = 0; wi(3) = -earth.o * si(lat*pi/180); wi = traspose(wi); The followig pieces of code were created: The tur rate of the NED frame with respect to the earth. The tur rate of the ody with respect to the NED frame i the ody frame. The skew matrix operatio. Update directioal cosie matrix fuctio. fuctio [we] = trlgf_earth(ve, v, lat, h) TRLGF_EARTH Fid the tur rate of the local geo frame with respect to the earth. [we] = trlgf_earth(velocity east, Velocity orth, Latitude, altitude) Latitude is i decimal degrees. Writte By: Bria Bleeker Ro MacMilla earth=earth_param; R = (earth.a(1-earth.e^2))/((1- earth.e^2*(si(lat*pi/180))^2)^(3/2)); Re = earth.a/((1-earth.e^2*(si(lat*pi/180))^2)^(1/2)); we(1) = ve/(re+h); we(2) = -v/(r+h); we(3) = (-ve*ta(lat*pi/180))/(re+h);

6 we = traspose(we); fuctio [w]=trbody_ned_body(wi,we,wi,c) TRBODY_NED_BODY Fid the tur rate of the vehicle with respect to the avigato frame i the ody frame. [w] = trbody_ned_body(tur rate of the earth w.r.t iertial frame i local geo frame, tur rate of local geo frame w.r.t the earth, raw agular data from IMU, Directioal cosie matrix) Writte By: Bria Bleeker Ro MacMilla w=wi-c*(we+wi); fuctio [omega] = skew(w) SKEW(WNB) Derive the skew matrix of w WNB = [w.x,w,y,wd.z] skew(w) 0 -w.z w.y w.z 0 -w.x -w.y w.x 0 Writte By: Bria Bleeker Ro MacMilla omega = [0,-w.z,w.y;w.z,0,-w.x;-w.y,w.x,0]; fuctio [c2]=update_c(c, delt, omega) UPDATE_CBN Update the directioal cosie matrix to compute ew attitude agles. [c] = update_c(c - old values, delta time, skew(w)) Writte By: Bria Bleeker Ro MacMilla c2 = c + delt[c*omega]; We also created a help file called avigatio to show what fuctios ca e used for IMU data maipulatio. fuctio avigatio earth_param - Returs the costat earth parameters to e used i other fuctios. ECEF2NED - Covert ecef coordiates to NED coordiates. NED2ECEF - Covert NED coordiates to ecef coordiates. skew - Derive the skew matrix of w

7 trlgf_earth - Fid the tur rate of the local geo frame with respect to earth. trbody_ned_body - Fid the tur rate of the vehicle i the avigatio frame with respect to the ody frame. trearth_if_ned - Fid the tur rate of the earth with respect to the iertial frame i the local NED frame. update_c - Update the directioal cosie matrix. Help fuctio - To fid more iformatio aout these fuctios.