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
Unified 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
% Get most recent GPS data that had fallen behind the fusion time horizon % 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' ); 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 ~isempty(latest_gps_index)
if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime)) % Check if GPS use is being blocked by the user
gps_use_started = false; if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime))
gps_use_blocked = true; gps_use_started = false;
else gps_use_blocked = true;
gps_use_blocked = false; else
end 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 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_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)) 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(5:7) = gps_data.vel_ned(latest_gps_index,:);
states(8:9) = gps_data.pos_ned(latest_gps_index,1:2); states(8:9) = gps_data.pos_ned(latest_gps_index,1:2);
gps_use_started = true; gps_use_started = true;
last_drift_constrain_time = local_time; last_drift_constrain_time = local_time;
end
end end
end
% Fuse GPS data when available if GPS use has started % Fuse GPS data when available if GPS use has started
if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index)) if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index))
last_gps_index = latest_gps_index; last_gps_index = latest_gps_index;
gps_fuse_index = gps_fuse_index + 1; gps_fuse_index = gps_fuse_index + 1;
last_drift_constrain_time = local_time; last_drift_constrain_time = local_time;
% fuse NED GPS velocity % 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)); [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 % data logging
output.innovations.vel_time_lapsed(gps_fuse_index) = local_time; output.innovations.vel_time_lapsed(gps_fuse_index) = local_time;
output.innovations.vel_innov(gps_fuse_index,:) = velInnov'; output.innovations.vel_innov(gps_fuse_index,:) = velInnov';
output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar'; output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
% fuse NE GPS position % 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)); [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 % data logging
output.innovations.pos_time_lapsed(gps_fuse_index) = local_time; output.innovations.pos_time_lapsed(gps_fuse_index) = local_time;
output.innovations.posInnov(gps_fuse_index,:) = posInnov'; output.innovations.posInnov(gps_fuse_index,:) = posInnov';
output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar'; output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar';
else 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 % 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_drift_constrain_time) > param.control.velDriftTimeLim)
if ((local_time - last_synthetic_velocity_fusion_time) > 0.2) if ((local_time - last_synthetic_velocity_fusion_time) > 0.2)
[states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim); [states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim);
last_synthetic_velocity_fusion_time = local_time; last_synthetic_velocity_fusion_time = local_time;
end
end end
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'
% function of the quaternions % function of the quaternions
Tbn_quat = Quat2Tbn([q0;q1;q2;q3]); 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)]; yaw_input_312 = [-Tbn_quat(1,2);Tbn_quat(2,2)];
% convert to c code and save % convert to c code and save

Write Preview
Loading…
Cancel
Save