1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/matlab/ins/sim.m
2012-04-03 03:42:23 -05:00

176 lines
5.7 KiB
Matlab

t0 = 60000;
bad = find(Gyros.timestamp > max(medfilt1(Gyros.timestamp,5)) | Gyros.timestamp < t0);
Gyros.timestamp(bad) = [];
Gyros.x(bad) = [];
Gyros.y(bad) = [];
Gyros.z(bad) = [];
Gyros.timestamp = medfilt1(Gyros.timestamp,5);
bad = find(Accels.timestamp > max(medfilt1(Accels.timestamp,5)));
Accels.timestamp(bad) = [];
Accels.x(bad) = [];
Accels.y(bad) = [];
Accels.z(bad) = [];
Accels.timestamp = medfilt1(Accels.timestamp,5);
bad = find(Magnetometer.timestamp > max(medfilt1(Magnetometer.timestamp,5)));
Magnetometer.timestamp(bad) = [];
Magnetometer.x(bad) = [];
Magnetometer.y(bad) = [];
Magnetometer.z(bad) = [];
Magnetometer.timestamp = medfilt1(Magnetometer.timestamp,5);
bad = find(BaroAltitude.timestamp > max(medfilt1(BaroAltitude.timestamp,5)));
BaroAltitude.timestamp(bad) = [];
BaroAltitude.Altitude(bad) = [];
bad = find(GPSPosition.timestamp > max(medfilt1(GPSPosition.timestamp,5)));
GPSPosition.timestamp(bad) = [];
GPSPosition.Latitude(bad) = [];
GPSPosition.Longitude(bad) = [];
GPSPosition.Altitude(bad) = [];
GPSPosition.Heading(bad) = [];
GPSPosition.Satellites(bad) = [];
GPSPosition.Groundspeed(bad) = [];
GPSPosition.timestamp = medfilt1(GPSPosition.timestamp,5);
outdoor = any(GPSPosition.Satellites > 7);
north = [24000 1700 43000];
baro_offset = 0;
if outdoor
idx = find(GPSPosition.Satellites > 7, 1, 'first');
home = [GPSPosition.Latitude(idx) GPSPosition.Longitude(idx) GPSPosition.Altitude(idx)];
home(1:2) = home(1:2) / 10e6;
baro_offset = -125;
end
insgps_ml('INSGPSInit')
insgps_ml('INSSetMagNorth',north);
insgps_ml('INSSetMagVar',[5 5 5]/1000);
insgps_ml('INSSetAccelVar',1.5e-5 * ones(1,3));
insgps_ml('INSSetGyroVar',2e-4*ones(1,3));
accel_idx = 1;
mag_idx = 1;
baro_idx = 1;
gps_idx = 1;
update_mag = false;
update_baro = false;
update_pos = false;
inited = false;
ned = zeros(3,length(Gyros.timestamp));
gps_vel = zeros(3,length(Gyros.timestamp));
a = zeros(13,length(Gyros.timestamp));
for i = 1:length(Gyros.timestamp)-1000
t = Gyros.timestamp(i);
gyro = [Gyros.x(i) Gyros.y(i) Gyros.z(i)] * pi / 180;
accel_idx = accel_idx - 1 + find(Accels.timestamp(accel_idx:end) >= t,1,'first');
accel = [Accels.x(accel_idx) Accels.y(accel_idx) Accels.z(accel_idx)-0.4];
dT = 0.0013;
[a(:,i) b K] = insgps_ml('INSStatePrediction',gyro,accel,dT);
if Magnetometer.timestamp(mag_idx) <= t
update_mag = true;
mag_idx = mag_idx + 1;
mag = [Magnetometer.x(mag_idx) Magnetometer.y(mag_idx) Magnetometer.z(mag_idx)];
end
if BaroAltitude.timestamp(baro_idx) <= t
baro_idx = baro_idx + 1;
update_baro = true;
baro = -BaroAltitude.Altitude(baro_idx) + baro_offset;
end
if outdoor && GPSPosition.timestamp(gps_idx) <= t
gps_idx = gps_idx + 1;
lla = [GPSPosition.Latitude(gps_idx) GPSPosition.Longitude(gps_idx) GPSPosition.Altitude(gps_idx)];
lla(1:2) = lla(1:2) / 10e6;
pos = LLA2NED(lla, home);
vel = [cos(GPSPosition.Heading(gps_idx) * pi / 180), sin(GPSPosition.Heading(gps_idx) * pi / 180) 0];
vel = vel * GPSPosition.Groundspeed(gps_idx);
update_pos = true;
elseif ~outdoor
pos = [0 0 baro];
vel = [0 0 0];
update_pos = true;
end
if t > 4.1e5 & t < 4.2e5
update_pos = false;
end
ned(:,i) = pos;
gps_vel(:,i) = vel;
if ~inited && update_pos && update_baro && update_mag
update_pos = false;
update_baro = false;
update_mag = false;
inited = true;
rpy(1) = atan2(accel(1), accel(3)) * 0;
rpy(2) = atan2(accel(2), accel(3)) * 0;
rpy(3) = atan2(mag(2),-mag(1));
%q1 = AttiudeFromVectors(mag',north',accel');
q1 = RPY2Quaternion(rpy * 180 / pi)';
insgps_ml('INSSetState',[pos(1) pos(2) baro vel q1' -mean(Gyros.x(1:100) * pi / 180) -mean(Gyros.y(1:100) * pi / 180) 0]);
elseif inited && i < 100
insgps_ml('INSSetMagVar',[5 5 5]);
insgps_ml('INSSetPosVelVar',1 / 1000);
% zero bias
%insgps_ml('INSSetState',[a(1:10,i)' 0 0 0]);
%state = insgps_ml('INSFullCorrection',mag,pos,vel,baro);
elseif inited
insgps_ml('INSSetMagVar',[5 5 5]*100);
if ~outdoor
if update_mag
insgps_ml('INSMagCorrection',mag);
update_mag = false;
elseif update_baro
insgps_ml('INSVelBaroCorrection',vel,baro);
update_baro = false;
elseif update_pos
insgps_ml('INSGpsCorrection',[0 0 baro],[0 0 0]);
update_pos = false;
end
else % outdoor
insgps_ml('INSSetPosVelVar',0.0001);
if update_mag
insgps_ml('INSMagCorrection',mag);
update_mag = false;
elseif update_baro
insgps_ml('INSBaroCorrection',baro);
update_baro = false;
elseif update_pos
insgps_ml('INSGpsCorrection',pos,vel);
update_pos = false;
end
end
end
%state = insgps_ml('INSFullCorrection',mag,pos,vel,baro);
if(mod(i,100) == 0)
q = a(7:10,1:i);
rpy = Quaternion2RPY(q');
subplot(311);
idx = find((AttitudeActual.timestamp < t) & (AttitudeActual.timestamp > Gyros.timestamp(1)));
plot(Gyros.timestamp(1:i),rpy(:,1:2),AttitudeActual.timestamp(idx),AttitudeActual.Roll(idx),'k',AttitudeActual.timestamp(idx),AttitudeActual.Pitch(idx),'k');
subplot(312);
plot(Gyros.timestamp(1:i), a(1:2,1:i)',Gyros.timestamp(1:i), ned(1:2,1:i)','k');
subplot(313);
plot(Gyros.timestamp(1:i), a(4:5,1:i)',Gyros.timestamp(1:i), gps_vel(1:2,1:i)','k');
drawnow
end
end