mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-04 10:21:35 +08:00
Matlab: Add basic implementation of EKF to support development testing
This commit is contained in:
Paul Riseborough
@@ -0,0 +1,19 @@
|
||||
function [T,sigma] = allan(omega,fs,pts)
|
||||
[N,M] = size(omega); % figure out how big the output data set is
|
||||
n = 2.^(0:floor(log2(N/2)))'; % determine largest bin size
|
||||
maxN = n(end);
|
||||
endLogInc = log10(maxN);
|
||||
m = unique(ceil(logspace(0,endLogInc,pts)))'; % create log spaced vector average factor
|
||||
t0 = 1/fs;
|
||||
T = m*t0;
|
||||
theta = cumsum(omega)/fs;
|
||||
sigma2 = zeros(length(T),M);
|
||||
for i=1:length(m)
|
||||
% t0 = sample interval
|
||||
% T = length of time for each cluster
|
||||
% integration of samples over time to obtain output angle ?
|
||||
% array of dimensions (cluster periods) X (#variables)
|
||||
% loop over the various cluster sizes
|
||||
% implements the summation in the AV equation
|
||||
sigma2 = sigma2./repmat((2*T.^2.*(N-2*m)),1,M);
|
||||
sigma = sqrt(sigma2)
|
||||
@@ -0,0 +1,196 @@
|
||||
% test ellipsoid sphere fitting algorithms
|
||||
%
|
||||
% http://www.st.com/content/ccc/resource/technical/document/design_tip/group0/a2/98/f5/d4/9c/48/4a/d1/DM00286302/files/DM00286302.pdf/jcr:content/translations/en.DM00286302.pdf
|
||||
%
|
||||
|
||||
%% load log data
|
||||
clear all;
|
||||
close all;
|
||||
|
||||
% uncomment these lines if using legacy .px4log format
|
||||
%load sysvector.mat;
|
||||
%mag_meas = [sysvector.IMU_MagX';sysvector.IMU_MagY';sysvector.IMU_MagZ'];
|
||||
|
||||
% uncomment these lines if using data imported from ulog format
|
||||
%load sysdata.mat;
|
||||
%mag_meas = [magnetometer_ga0';magnetometer_ga1';magnetometer_ga2'];
|
||||
|
||||
% direct loading of .px4log files
|
||||
ecl_path = '/Users/paul/src/pacflyer_PX4/PX4/src/lib/ecl/matlab/analysis';
|
||||
addpath ecl_path;
|
||||
data1 = importPX4log('/Users/paul/Downloads/20170507_1013_sess006.px4log','IMU');
|
||||
data2 = importPX4log('/Users/paul/Downloads/20170507_1013_sess007.px4log','IMU');
|
||||
data3 = importPX4log('/Users/paul/Downloads/20170507_1013_sess008.px4log','IMU');
|
||||
|
||||
% thin data points to use data every 5 deg
|
||||
delta_angle_lim = 5* pi/180;
|
||||
counter = 1;
|
||||
|
||||
angle = 0;
|
||||
last_angle = 0;
|
||||
for i = 2:length(data1.IMU.Tsec)
|
||||
ang_rate = 0.5 * (sqrt(data1.IMU.GyroX(i)^2 + data1.IMU.GyroY(i)^2 + data1.IMU.GyroZ(i)^2) + ...
|
||||
sqrt(data1.IMU.GyroX(i-1)^2 + data1.IMU.GyroY(i-1)^2 + data1.IMU.GyroZ(i-1)^2));
|
||||
dt = data1.IMU.Tsec(i) - data1.IMU.Tsec(i-1);
|
||||
angle = angle + ang_rate * dt;
|
||||
if ((angle - last_angle) > delta_angle_lim)
|
||||
mag_meas(:,counter) = [data1.IMU.MagX(i);data1.IMU.MagY(i);data1.IMU.MagZ(i)];
|
||||
counter = counter + 1;
|
||||
last_angle = angle;
|
||||
end
|
||||
end
|
||||
|
||||
angle = 0;
|
||||
last_angle = 0;
|
||||
for i = 2:length(data2.IMU.Tsec)
|
||||
ang_rate = 0.5 * (sqrt(data2.IMU.GyroX(i)^2 + data2.IMU.GyroY(i)^2 + data2.IMU.GyroZ(i)^2) + ...
|
||||
sqrt(data2.IMU.GyroX(i-1)^2 + data2.IMU.GyroY(i-1)^2 + data2.IMU.GyroZ(i-1)^2));
|
||||
dt = data2.IMU.Tsec(i) - data2.IMU.Tsec(i-1);
|
||||
angle = angle + ang_rate * dt;
|
||||
if ((angle - last_angle) > delta_angle_lim)
|
||||
mag_meas(:,counter) = [data2.IMU.MagX(i);data2.IMU.MagY(i);data2.IMU.MagZ(i)];
|
||||
counter = counter + 1;
|
||||
last_angle = angle;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
angle = 0;
|
||||
last_angle = 0;
|
||||
for i = 2:length(data3.IMU.Tsec)
|
||||
ang_rate = 0.5 * (sqrt(data3.IMU.GyroX(i)^2 + data3.IMU.GyroY(i)^2 + data3.IMU.GyroZ(i)^2) + ...
|
||||
sqrt(data3.IMU.GyroX(i-1)^2 + data3.IMU.GyroY(i-1)^2 + data3.IMU.GyroZ(i-1)^2));
|
||||
dt = data3.IMU.Tsec(i) - data3.IMU.Tsec(i-1);
|
||||
angle = angle + ang_rate * dt;
|
||||
if ((angle - last_angle) > delta_angle_lim)
|
||||
mag_meas(:,counter) = [data3.IMU.MagX(i);data3.IMU.MagY(i);data3.IMU.MagZ(i)];
|
||||
counter = counter + 1;
|
||||
last_angle = angle;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
|
||||
%% fit a sphere and determine the fit quality
|
||||
[offset,gain,rotation]=ellipsoid_fit(mag_meas',5);
|
||||
|
||||
% correct the data
|
||||
mag_corrected_5 = zeros(size(mag_meas));
|
||||
rotation_correction = inv(rotation); % we apply the inverse of the original rotation
|
||||
scale_correction = 1./gain;
|
||||
scale_correction = scale_correction ./ mean(scale_correction);
|
||||
mag_strength = zeros(length(mag_meas),1);
|
||||
for i = 1:length(mag_meas)
|
||||
% subtract the offsets
|
||||
mag_corrected_5(:,i) = mag_meas(:,i) - offset;
|
||||
|
||||
% correct the rotation
|
||||
mag_corrected_5(:,i) = rotation_correction * mag_corrected_5(:,i);
|
||||
|
||||
% correct the scale factor
|
||||
mag_corrected_5(:,i) = mag_corrected_5(:,i) .* scale_correction;
|
||||
|
||||
% calculate the residual
|
||||
mag_strength(i) = sqrt(dot(mag_corrected_5(:,i),mag_corrected_5(:,i)));
|
||||
|
||||
end
|
||||
|
||||
% calculate the fit residual for fit option 5
|
||||
fit_residual_5 = mag_strength - mean(mag_strength);
|
||||
|
||||
%% fit a un-rotated ellipsoid and determine the fit quality
|
||||
[offset,gain,rotation]=ellipsoid_fit(mag_meas',1);
|
||||
|
||||
% correct the data
|
||||
mag_corrected_1 = zeros(size(mag_meas));
|
||||
rotation_correction = inv(rotation); % we apply the inverse of the original rotation
|
||||
scale_correction = 1./gain;
|
||||
scale_correction = scale_correction ./ mean(scale_correction);
|
||||
mag_strength = zeros(length(mag_meas),1);
|
||||
angle_change_1 = zeros(length(mag_meas),1);
|
||||
for i = 1:length(mag_meas)
|
||||
% subtract the offsets
|
||||
mag_corrected_1(:,i) = mag_meas(:,i) - offset;
|
||||
|
||||
% correct the rotation
|
||||
mag_corrected_1(:,i) = rotation_correction * mag_corrected_1(:,i);
|
||||
|
||||
% correct the scale factor
|
||||
mag_corrected_1(:,i) = mag_corrected_1(:,i) .* scale_correction;
|
||||
|
||||
% calculate the residual
|
||||
mag_strength(i) = sqrt(dot(mag_corrected_1(:,i),mag_corrected_1(:,i)));
|
||||
|
||||
% calculate the angular change due to the fit
|
||||
angle_change_1(i) = atan2(norm(cross(mag_corrected_1(:,i),mag_meas(:,i))),dot(mag_corrected_1(:,i),mag_meas(:,i)));
|
||||
|
||||
end
|
||||
|
||||
% calculate the fit residual for fit option 1
|
||||
fit_residual_1 = mag_strength - mean(mag_strength);
|
||||
|
||||
%% fit a rotated ellipsoid and check the fit quality
|
||||
[offset,gain,rotation]=ellipsoid_fit(mag_meas',0);
|
||||
|
||||
% correct the data
|
||||
mag_corrected_0 = zeros(size(mag_meas));
|
||||
rotation_correction = inv(rotation); % we apply the inverse of the original rotation
|
||||
scale_correction = 1./gain;
|
||||
scale_correction = scale_correction ./ mean(scale_correction);
|
||||
mag_strength = zeros(length(mag_meas),1);
|
||||
angle_change_0 = zeros(length(mag_meas),1);
|
||||
for i = 1:length(mag_meas)
|
||||
% subtract the offsets
|
||||
mag_corrected_0(:,i) = mag_meas(:,i) - offset;
|
||||
|
||||
% correct the rotation
|
||||
mag_corrected_0(:,i) = rotation_correction * mag_corrected_0(:,i);
|
||||
|
||||
% correct the scale factor
|
||||
mag_corrected_0(:,i) = mag_corrected_0(:,i) .* scale_correction;
|
||||
|
||||
% calculate the residual
|
||||
mag_strength(i) = sqrt(dot(mag_corrected_0(:,i),mag_corrected_0(:,i)));
|
||||
|
||||
% calculate the angular change due to the fit
|
||||
angle_change_0(i) = atan2(norm(cross(mag_corrected_0(:,i),mag_meas(:,i))),dot(mag_corrected_0(:,i),mag_meas(:,i)));
|
||||
|
||||
end
|
||||
|
||||
% calculate the fit residual for fit option 0
|
||||
fit_residual_0 = mag_strength - mean(mag_strength);
|
||||
|
||||
%% calculate the residual for uncorrected data
|
||||
for i = 1:length(mag_meas)
|
||||
mag_strength(i) = sqrt(dot(mag_meas(:,i),mag_meas(:,i)));
|
||||
end
|
||||
uncorrected_residual = mag_strength - mean(mag_strength);
|
||||
|
||||
%% plot the fit residuals
|
||||
plot(uncorrected_residual,'k+');
|
||||
hold on;
|
||||
plot(fit_residual_5,'r+');
|
||||
plot(fit_residual_1,'b+');
|
||||
plot(fit_residual_0,'g+');
|
||||
hold off;
|
||||
grid on;
|
||||
title('mag fit comparison');
|
||||
xlabel('measurement index');
|
||||
ylabel('fit residual (Gauss)');
|
||||
legend('uncorrected','sphere','non-rotated ellipse','rotated ellipse');
|
||||
|
||||
%% plot the data points in 3D
|
||||
figure;
|
||||
plot3(mag_meas(1,:),mag_meas(2,:),mag_meas(3,:),' .');hold on;plot3(mag_corrected_1(1,:),mag_corrected_1(2,:),mag_corrected_1(3,:),'r.');
|
||||
hold off;grid on;axis equal;
|
||||
xlabel('x (Gauss)');
|
||||
xlabel('y (Gauss)');
|
||||
xlabel('z (Gauss)');
|
||||
legend('uncorrected','unrotated ellipse');
|
||||
|
||||
%% calculate and plot the angular error
|
||||
figure;
|
||||
plot(angle_change_1*(180/pi),'b+');
|
||||
title('angle change after un-rotated ellipse fit');
|
||||
xlabel('measurement index');
|
||||
ylabel('angle change magnitude (deg)');
|
||||
@@ -0,0 +1,61 @@
|
||||
function [ofs,gain,rotM]=ellipsoid_fit(XYZ,varargin)
|
||||
|
||||
% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points
|
||||
|
||||
% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z)
|
||||
|
||||
% optional flag f, default to 0 (fitting of rotated ellipsoid)
|
||||
|
||||
x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3); if nargin>1, f=varargin{1}; else f=0; end;
|
||||
|
||||
if f==0, D=[x.*x, y.*y, z.*z, 2*x.*y,2*x.*z,2*y.*z, 2*x,2*y,2*z]; % any axes (rotated ellipsoid)
|
||||
|
||||
elseif f==1, D=[x.*x, y.*y, z.*z, 2*x,2*y,2*z]; % XYZ axes (non-rotated ellipsoid)
|
||||
|
||||
elseif f==2, D=[x.*x+y.*y, z.*z, 2*x,2*y,2*z]; % and radius x=y
|
||||
|
||||
elseif f==3, D=[x.*x+z.*z, y.*y, 2*x,2*y,2*z]; % and radius x=z
|
||||
|
||||
elseif f==4, D=[y.*y+z.*z, x.*x, 2*x,2*y,2*z]; % and radius y=z
|
||||
|
||||
elseif f==5, D=[x.*x+y.*y+z.*z, 2*x,2*y,2*z]; % and radius x=y=z (sphere)
|
||||
|
||||
end;
|
||||
|
||||
v = (D'*D)\(D'*ones(length(x),1)); % least square fitting
|
||||
|
||||
if f==0, % rotated ellipsoid
|
||||
|
||||
A = [ v(1) v(4) v(5) v(7); v(4) v(2) v(6) v(8); v(5) v(6) v(3) v(9); v(7) v(8) v(9) -1 ];
|
||||
|
||||
ofs=-A(1:3,1:3)\[v(7);v(8);v(9)]; % offset is center of ellipsoid
|
||||
|
||||
Tmtx=eye(4); Tmtx(4,1:3)=ofs'; AT=Tmtx*A*Tmtx'; % ellipsoid translated to (0,0,0)
|
||||
|
||||
[rotM ev]=eig(AT(1:3,1:3)/-AT(4,4)); % eigenvectors (rotation) and eigenvalues (gain)
|
||||
|
||||
gain=sqrt(1./diag(ev)); % gain is radius of the ellipsoid
|
||||
|
||||
else % non-rotated ellipsoid
|
||||
|
||||
if f==1, v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
|
||||
|
||||
elseif f==2, v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
|
||||
|
||||
elseif f==3, v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
|
||||
|
||||
elseif f==4, v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
|
||||
|
||||
elseif f==5, v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; % sphere
|
||||
|
||||
end;
|
||||
|
||||
ofs=-(v(1:3).\v(7:9))'; % offset is center of ellipsoid
|
||||
|
||||
rotM=eye(3); % eigenvectors (rotation), identity = no rotation
|
||||
|
||||
g=1+(v(7)^2/v(1)+v(8)^2/v(2)+v(9)^2/v(3));
|
||||
|
||||
gain=(sqrt(g./v(1:3)))'; % find radii of the ellipsoid (scale)
|
||||
|
||||
end;
|
||||
Reference in New Issue
Block a user