fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 08:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

matlab: fix alignment bugs

Browse Source
This commit is contained in:
Paul Riseborough
2017-06-17 11:46:36 +10:00
parent 7f1cca5b28
commit edeed770b7
3 changed files with 19 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matlab/EKF_replay/Filter/AlignHeading.m
+14 -38
View File
@@ -1,47 +1,23 @@
function quat = AlignHeading( ... function quat = AlignHeading( ...
quat, ... % quaternion state vector quat, ... % quaternion state vector
magMea, ... % body frame magnetic flux measurements magMea, ... % body frame magnetic flux measurements
declination) % Estimated magnetic field delination at current location declination) % Estimated magnetic field delination at current location (rad)
% Calculate the predicted magnetic declination % Get the Euler angles and set yaw to zero
Tbn = Quat2Tbn(quat); euler = QuatToEul(quat);
magMeasNED = Tbn*magMea; euler(3) = 0.0;
predDec = atan2(magMeasNED(2),magMeasNED(1));
% Calculate the measurement innovation % calculate the rotation matrix from body to earth frame
innovation = predDec - declination; quat_zero_yaw = EulToQuat(euler);
Tbn_zero_yaw = Quat2Tbn(quat_zero_yaw);
if (innovation > pi) % Rotate the magnetic field measurement into earth frame
innovation = innovation - 2*pi; magMeasNED = Tbn_zero_yaw*magMea;
elseif (innovation < -pi)
innovation = innovation + 2*pi;
end
% form the NED rotation vector % Use the projection onto the horizontal to calculate the yaw angle
deltaRotNED = -[0;0;innovation]; euler(3) = declination - atan2(magMeasNED(2),magMeasNED(1));
% rotate into body axes % convert to a quaternion
% Calculate the body to nav cosine matrix quat = EulToQuat(euler);
Tbn = Quat2Tbn(quat);
deltaRotBody = transpose(Tbn)*deltaRotNED;
% Convert the error rotation vector to its equivalent quaternion
% error = truth - estimate
rotationMag = abs(innovation);
if rotationMag<1e-6
deltaQuat = single([1;0;0;0]);
else
deltaQuat = [cos(0.5*rotationMag); [deltaRotBody(1);deltaRotBody(2);deltaRotBody(3)]/rotationMag*sin(0.5*rotationMag)];
end
% Update the quaternion states by rotating from the previous attitude through
% the delta angle rotation quaternion
quat = [quat(1)*deltaQuat(1)-transpose(quat(2:4))*deltaQuat(2:4); quat(1)*deltaQuat(2:4) + deltaQuat(1)*quat(2:4) + cross(quat(2:4),deltaQuat(2:4))];
% normalise the updated quaternion states
quatMag = sqrt(quat(1)^2 + quat(2)^2 + quat(3)^2 + quat(4)^2);
if (quatMag > 1e-12)
quat = quat / quatMag;
end
end end
matlab/EKF_replay/Filter/InitStates.m
+4 -3
View File
@@ -16,9 +16,9 @@ else
end end
% average first 100 accel readings to reduce effect of vibration % average first 100 accel readings to reduce effect of vibration
initAccel(1) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,1)); initAccel(1) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,1))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
initAccel(2) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,2)); initAccel(2) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,2))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3)); initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1));
% align tilt using gravity vector (If the velocity is changing this will % align tilt using gravity vector (If the velocity is changing this will
% induce errors) % induce errors)
@@ -36,6 +36,7 @@ magBody(3,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,3));
% align heading and initialise the NED magnetic field states % align heading and initialise the NED magnetic field states
quat = AlignHeading(quat,magBody,param.fusion.magDeclDeg*deg2rad); quat = AlignHeading(quat,magBody,param.fusion.magDeclDeg*deg2rad);
states(1:4) = quat;
% initialise the NED magnetic field states % initialise the NED magnetic field states
Tbn = Quat2Tbn(quat); Tbn = Quat2Tbn(quat);
matlab/EKF_replay/Filter/SetParameterDefaults.m
+1 -1
View File
@@ -51,7 +51,7 @@ param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consi
param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec) param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec)
param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec) param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec)
param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2) param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2)
param.prediction.dVelBiasPnoise = 0.003; % IMU accel bias 1SD rate of change (m/sec^3) param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3)
param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec) param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec)
param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2) param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2)
param.prediction.windPnoiseNE = 0.1; % wind state process 1SD rate of change (m/sec^2) param.prediction.windPnoiseNE = 0.1; % wind state process 1SD rate of change (m/sec^2)