mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-05 13:24:11 +01:00
176 lines
5.7 KiB
Matlab
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
|