allan 方差

%%matlab function

function [acc_sig,acc_wk] = allan_var(data,t0)
%ALLAN_VAR 此处显示有关此函数的摘要
%   此处显示详细说明
acc_data = data(:,1:3);
acc_sig = zeros(3,3);
acc_wk = zeros(3,3);

for i=1:3
    [adev,tau]=calc_var(acc_data(:,i),t0);
    slope = -0.5;
    b = search_by_slope(adev,tau,slope);
    % Determine the angle random walk coefficient from the line.
    logN = slope*log(1) + b;
    N = 10^logN;
    acc_sig(i,i) = N;
    
    %%walk noise
    slope = 0.5;
    [adev,tau]=calc_var(acc_data(:,i),t0);
    b = search_by_slope(adev,tau,slope);
    logK = slope*log10(3) + b;
    K = 10^logK;
    acc_wk(i,i)= K;
end

end

function b = search_by_slope(adev,tau,slope)

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);
end

function [adev,tau]=calc_var(omega,t0)

    theta = cumsum(omega, 1)*t0;
    maxNumM = 100;
    L = size(theta, 1);
    maxM = 2.^floor(log2(L/2));
    m = logspace(log10(1), 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));

    adev = sqrt(avar);
end

%%-------------------------demo-------------------------------------

t0 = 0.005;
rawdata = load('D:\\project\\cpp\\imu_prei\\create_virtual_imu_data/imu_pose_noise.txt');
%rawdata = load('D:\\project\\cpp\\course2_hw_new\\vio_data_simulation\\build/imudata.txt');t0 = 1.0/200;
N_sample = size(rawdata,1);

data = rawdata(:,9:14);

[acc_noise,acc_walk]=allan_var(data(:,4:6),t0)
[gyr_noise,gyr_walk]=allan_var(data(:,1:3),t0)
disp('-----------------------------------------------')

return;