forked from ZhanLi/UWBIns
108 lines
2.3 KiB
Matlab
108 lines
2.3 KiB
Matlab
|
||
%% ('NoiseDensity(角度随机游走)', N, 'RandomWalk(角速率随机游走)', K,'BiasInstability(零偏稳定性)', B);
|
||
% omega 必须为 rad/s(陀螺)， 加速度计: m/s^(2)
|
||
% see Freescale AN: Allan Variance: Noise Analysis for Gyroscopes
|
||
function [avar, tau, N, K, B] = allan(omega, Fs)
|
||
|
||
% t0 = 1/Fs;
|
||
% theta = cumsum(omega, 1)*t0;
|
||
%
|
||
% maxNumM = 200;
|
||
% L = size(theta, 1);
|
||
% maxM = 2.^floor(log2(L/2));
|
||
% m = logspace(log10(10), log10(maxM), maxNumM).';
|
||
% m = ceil(m); % m must be an integer.
|
||
% m = unique(m); % Remove duplicates.
|
||
%
|
||
% tau = m*t0;
|
||
%
|
||
% avar = zeros(numel(m), 1);
|
||
% for i = 1:numel(m)
|
||
% mi = m(i);
|
||
% avar(i,:) = sum( ...
|
||
% (theta(1+2*mi:L) - 2*theta(1+mi:L-mi) + theta(1:L-2*mi)).^2, 1);
|
||
% end
|
||
% avar = avar ./ (2*tau.^2 .* (L - 2*m));
|
||
|
||
[avar,tau] = allanvar(omega, 'octave', Fs);
|
||
adev = sqrt(avar);
|
||
|
||
|
||
%% Angle Random Walk
|
||
slope = -0.5;
|
||
logtau = log10(tau);
|
||
logadev = log10(adev);
|
||
dlogadev = diff(logadev) ./ diff(logtau);
|
||
[~, i] = min(abs(dlogadev - slope));
|
||
|
||
% Find the y-intercept of the line.
|
||
b = logadev(i) - slope*logtau(i);
|
||
|
||
% Determine the angle random walk coefficient from the line.
|
||
logN = slope*log10(1) + b;
|
||
N = 10^logN;
|
||
|
||
% Plot the results.
|
||
tauN = 1;
|
||
lineN = N ./ sqrt(tau);
|
||
|
||
|
||
|
||
%% Rate Random Walk
|
||
slope = 0.5;
|
||
logtau = log10(tau);
|
||
logadev = log10(adev);
|
||
dlogadev = diff(logadev) ./ diff(logtau);
|
||
[~, i] = min(abs(dlogadev - slope));
|
||
|
||
% Find the y-intercept of the line.
|
||
b = logadev(i) - slope*logtau(i);
|
||
|
||
% Determine the rate random walk coefficient from the line.
|
||
logK = slope*log10(3) + b;
|
||
K = 10^logK;
|
||
|
||
% Plot the results.
|
||
tauK = 3;
|
||
lineK = K .* sqrt(tau/3);
|
||
|
||
|
||
|
||
|
||
%% Bias Instability
|
||
slope = 0;
|
||
logtau = log10(tau);
|
||
logadev = log10(adev);
|
||
dlogadev = diff(logadev) ./ diff(logtau);
|
||
|
||
[~, i] = min(abs(dlogadev - slope));
|
||
[~, i] = min(adev); %yandld added
|
||
|
||
% Find the y-intercept of the line.
|
||
b = logadev(i) - slope*logtau(i);
|
||
|
||
% Determine the bias instability coefficient from the line.
|
||
scfB = sqrt(2*log(2)/pi);
|
||
logB = b - log10(scfB);
|
||
B = 10^logB;
|
||
|
||
% Plot the results.
|
||
tauB = tau(i);
|
||
lineB = B * scfB * ones(size(tau));
|
||
|
||
|
||
|
||
|
||
tauParams = [tauN, tauK, tauB];
|
||
params = [N, K, scfB*B];
|
||
figure;
|
||
loglog(tau, adev, '-*', tau, [lineN, lineK, lineB], '--', tauParams, params, 'o');
|
||
|
||
title('Allan Deviation with Noise Parameters')
|
||
xlabel('\tau')
|
||
ylabel('\sigma(\tau)')
|
||
legend('\sigma', '\sigma_N', '\sigma_K', '\sigma_B')
|
||
text(tauParams, params, {'N', 'K', '0.664B'})
|
||
grid on
|
||
|
||
end |