fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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:28 +10:00
parent 7f1cca5b28
commit edeed770b7
3 changed files with 19 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
matlab/EKF_replay/Filter/AlignHeading.m
View File

@ -1,47 +1,23 @@
function quat = AlignHeading( ...
quat, ... % quaternion state vector
magMea, ... % body frame magnetic flux measurements
declination) % Estimated magnetic field delination at current location
magMea, ... % body frame magnetic flux measurements
declination) % Estimated magnetic field delination at current location (rad)
% Calculate the predicted magnetic declination
Tbn = Quat2Tbn(quat);
magMeasNED = Tbn*magMea;
predDec = atan2(magMeasNED(2),magMeasNED(1));
% Get the Euler angles and set yaw to zero
euler = QuatToEul(quat);
euler(3) = 0.0;
% Calculate the measurement innovation
innovation = predDec - declination;
% calculate the rotation matrix from body to earth frame
quat_zero_yaw = EulToQuat(euler);
Tbn_zero_yaw = Quat2Tbn(quat_zero_yaw);
if (innovation > pi)
innovation = innovation - 2*pi;
elseif (innovation < -pi)
innovation = innovation + 2*pi;
end
% Rotate the magnetic field measurement into earth frame
magMeasNED = Tbn_zero_yaw*magMea;
% form the NED rotation vector
deltaRotNED = -[0;0;innovation];
% Use the projection onto the horizontal to calculate the yaw angle
euler(3) = declination - atan2(magMeasNED(2),magMeasNED(1));
% rotate into body axes
% Calculate the body to nav cosine matrix
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
% convert to a quaternion
quat = EulToQuat(euler);
end

7
matlab/EKF_replay/Filter/InitStates.m
View File

@ -16,9 +16,9 @@ else
end
% 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(2) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,2));
initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3));
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))./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))./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
% 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
quat = AlignHeading(quat,magBody,param.fusion.magDeclDeg*deg2rad);
states(1:4) = quat;
% initialise the NED magnetic field states
Tbn = Quat2Tbn(quat);

2
matlab/EKF_replay/Filter/SetParameterDefaults.m
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.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.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.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)