From edeed770b7e2a2843aa87a8cc1a74b53b865dbfc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 17 Jun 2017 11:46:28 +1000 Subject: [PATCH] matlab: fix alignment bugs --- matlab/EKF_replay/Filter/AlignHeading.m | 52 +++++-------------- matlab/EKF_replay/Filter/InitStates.m | 7 +-- .../EKF_replay/Filter/SetParameterDefaults.m | 2 +- 3 files changed, 19 insertions(+), 42 deletions(-) diff --git a/matlab/EKF_replay/Filter/AlignHeading.m b/matlab/EKF_replay/Filter/AlignHeading.m index 00f636cee6..3b3862b6e5 100644 --- a/matlab/EKF_replay/Filter/AlignHeading.m +++ b/matlab/EKF_replay/Filter/AlignHeading.m @@ -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 \ No newline at end of file diff --git a/matlab/EKF_replay/Filter/InitStates.m b/matlab/EKF_replay/Filter/InitStates.m index 69b4762082..1687f64b23 100644 --- a/matlab/EKF_replay/Filter/InitStates.m +++ b/matlab/EKF_replay/Filter/InitStates.m @@ -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); diff --git a/matlab/EKF_replay/Filter/SetParameterDefaults.m b/matlab/EKF_replay/Filter/SetParameterDefaults.m index 2fbbc01836..75ee723971 100644 --- a/matlab/EKF_replay/Filter/SetParameterDefaults.m +++ b/matlab/EKF_replay/Filter/SetParameterDefaults.m @@ -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)