zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

matlab: Allow replay to handle late start GPS data in log 

Also minor fix to comments.
master
Paul Riseborough 8 years ago
parent
e18f92d9c2
commit
33e8d5923f
2 changed files with 45 additions and 43 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 78
      matlab/EKF_replay/Filter/RunFilter.m
  2. 2
      matlab/scripts/Inertial Nav EKF/quat2yaw312.m

78
matlab/EKF_replay/Filter/RunFilter.m
Unescape View File

@ -158,51 +158,53 @@ for index = indexStart:indexStop @@ -158,51 +158,53 @@ for index = indexStart:indexStop
% Get most recent GPS data that had fallen behind the fusion time horizon
latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
% Check if GPS use is being blocked by the user
if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime))
gps_use_started = false;
gps_use_blocked = true;
else
gps_use_blocked = false;
end
if ~isempty(latest_gps_index)
% Check if GPS use is being blocked by the user
if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime))
gps_use_started = false;
gps_use_blocked = true;
else
gps_use_blocked = false;
end
% If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS
if (~gps_use_started && ~gps_use_blocked)
if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim))
states(5:7) = gps_data.vel_ned(latest_gps_index,:);
states(8:9) = gps_data.pos_ned(latest_gps_index,1:2);
gps_use_started = true;
last_drift_constrain_time = local_time;
% If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS
if (~gps_use_started && ~gps_use_blocked)
if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim))
states(5:7) = gps_data.vel_ned(latest_gps_index,:);
states(8:9) = gps_data.pos_ned(latest_gps_index,1:2);
gps_use_started = true;
last_drift_constrain_time = local_time;
end
end
end
% Fuse GPS data when available if GPS use has started
if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index))
last_gps_index = latest_gps_index;
gps_fuse_index = gps_fuse_index + 1;
last_drift_constrain_time = local_time;
% Fuse GPS data when available if GPS use has started
if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index))
last_gps_index = latest_gps_index;
gps_fuse_index = gps_fuse_index + 1;
last_drift_constrain_time = local_time;
% fuse NED GPS velocity
[states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index));
% fuse NED GPS velocity
[states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index));
% data logging
output.innovations.vel_time_lapsed(gps_fuse_index) = local_time;
output.innovations.vel_innov(gps_fuse_index,:) = velInnov';
output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
% data logging
output.innovations.vel_time_lapsed(gps_fuse_index) = local_time;
output.innovations.vel_innov(gps_fuse_index,:) = velInnov';
output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
% fuse NE GPS position
[states,covariance,posInnov,posInnovVar] = FusePosition(states,covariance,gps_data.pos_ned(latest_gps_index,:),param.fusion.gpsPosGate,gps_data.pos_error(latest_gps_index));
% fuse NE GPS position
[states,covariance,posInnov,posInnovVar] = FusePosition(states,covariance,gps_data.pos_ned(latest_gps_index,:),param.fusion.gpsPosGate,gps_data.pos_error(latest_gps_index));
% data logging
output.innovations.pos_time_lapsed(gps_fuse_index) = local_time;
output.innovations.posInnov(gps_fuse_index,:) = posInnov';
output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar';
else
% Check if drift is being corrected by some form of aiding and if not, fuse in a zero position measurement at 5Hz to prevent states diverging
if ((local_time - last_drift_constrain_time) > param.control.velDriftTimeLim)
if ((local_time - last_synthetic_velocity_fusion_time) > 0.2)
[states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim);
last_synthetic_velocity_fusion_time = local_time;
% data logging
output.innovations.pos_time_lapsed(gps_fuse_index) = local_time;
output.innovations.posInnov(gps_fuse_index,:) = posInnov';
output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar';
else
% Check if drift is being corrected by some form of aiding and if not, fuse in a zero position measurement at 5Hz to prevent states diverging
if ((local_time - last_drift_constrain_time) > param.control.velDriftTimeLim)
if ((local_time - last_synthetic_velocity_fusion_time) > 0.2)
[states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim);
last_synthetic_velocity_fusion_time = local_time;
end
end
end
end

2
matlab/scripts/Inertial Nav EKF/quat2yaw312.m
Unescape View File

@ -21,7 +21,7 @@ syms q0 q1 q2 q3 'real' @@ -21,7 +21,7 @@ syms q0 q1 q2 q3 'real'
% function of the quaternions
Tbn_quat = Quat2Tbn([q0;q1;q2;q3]);
% calculate the y,x terms required for calculation fo the yaw angle
% collect the y,x terms required for calculation of the yaw angle
yaw_input_312 = [-Tbn_quat(1,2);Tbn_quat(2,2)];
% convert to c code and save

Write Preview
Loading…
Cancel
Save