惯性导航学习过程记录（allan 方差）

碎碎念：今天用下面的代码分析了组用神经网络对IMU去噪的数据，发现得到的allan方差图都是倒着的对勾，才刚刚想起来allan方差是用来处理静态数据的，动态数据好像不太行。

一个matlab 例程：

clear;
clc;
close all;

%% : read matlabe internal dataset 
% load('LoggedSingleAxisGyroscope', 'omega', 'Fs')
% gyroReading = omega;

%% ADIS16488 dataset
% load('adis16488_gyr.mat');
% gyroReading = omega(:,1);
% gyroReading = deg2rad(gyroReading);

%%  ch00   deg/s    m/s^(2)

load('ch100.mat');
gyroReading = gyroReading(:,2);
gyroReading = deg2rad(gyroReading);
accelReading = accelReading(:,2);
accelReading = accelReading *  9.8066;
Fs = 400;


%% 
% data = ch_data_import('UranusData.csv');


% VRW 单位:                    m/s/sqrt(hr)           'accel_vrw': np.array([0.3119, 0.6009, 0.9779]) * 1.0,
% 加速度零偏不稳定性:     m/s^(2)                 'accel_b_stability': np.array([1e-3, 3e-3, 5e-3]) * 1.0,
% ARW 单位:                    deg/sqrt(hr)           'gyro_arw': np.array([0.25, 0.25, 0.25]) * 1.0,
% 角速度零偏稳定性:        deg/h                     'gyro_b_stability': np.array([3.5, 3.5, 3.5]) * 1.0,  

% M = csvread('gyro-0.csv',1 ,0);
% gyroReading = M(:,1);
% gyroReading = deg2rad(gyroReading); 
% 
% M = csvread('accel-0.csv',1 ,0);
% Fs = 100;
% accelReading = M(:,1);


%% 陀螺仿真 , 仿真输出单位为 rad/s
% B = 0.00203140909966965;
% N = 0.0125631765533906;
% K = 9.38284069320333e-05;
% L =2160000;
% Fs = 100;
% 
% 
% gyro = gyroparams('NoiseDensity', N, 'RandomWalk', K,'BiasInstability', B);
% acc = zeros(L, 3);
% angvel = zeros(L, 3);
% imu = imuSensor('SampleRate', Fs, 'Gyroscope', gyro);
% [~, omega ] = imu(acc, angvel);
% omega  = omega (:,1);
 

 %% 加计仿真, 仿真输出单位为 m/s^(2)
% B = 0.00203140909966965;
% N = 0.0125631765533906;
% K = 9.38284069320333e-05;
% L =2160000;
% Fs = 100;
% 
% 
% SpecAcc = accelparams('NoiseDensity', N, 'RandomWalk', K,'BiasInstability', B);
% acc = zeros(L, 3);
% angvel = zeros(L, 3);
% imu = imuSensor('SampleRate', Fs, 'Accelerometer', SpecAcc);
% [accelReading, ~] = imu(acc, angvel);
% accelReading = accelReading(:,1);


            
%% 运行陀螺 allan
[avar1, tau1 , N, K, B] = ch_allan(gyroReading , Fs, true);
fprintf('GYR: 零偏不稳定性                                                             %frad/s                    或   %fdeg/h \n', B, rad2deg(B)*3600);
fprintf('GYR: 噪声密度(角度随机游走, ARW, Noise density)              %f(rad/s)/sqrt(Hz)     或  %f deg/sqrt(h)\n', N, rad2deg(N)*3600^(0.5));
fprintf('GYR: 角速率随机游走, bias variations ("random walks")       %f(rad/s)sqrt(Hz)       或  %f deg/h/sqrt(h)\n', K, rad2deg(K) * (3600^(1.5)));



%% 运行加速度计 allan
[avar1, tau1 , N, K, B] = ch_allan(accelReading, Fs, true);

fprintf('ACC: 零偏不稳定性                                                                                       %fm/s^(2)                       或   %fmg  或  %fug\n', B, B / 9.80665 *1000,  B / 9.80665 *1000*1000);
fprintf('ACC: 噪声密度(速率随机游走,VRW, Noise Density, Rate Noise Density)          %f(m/s^(2))/sqrt(Hz)        或   %f m/s/sqrt(hr)\n', N, N * 3600^(0.5));
fprintf('ACC: 加速度随机游走，bias variations ("random walks")                               %f(m/s^(2)sqrt(Hz))\n',  K);

%%  ('NoiseDensity(角度随机游走)', N, 'RandomWalk(角速率随机游走)', K,'BiasInstability(零偏稳定性)', B);
% omega 必须为 rad/s
% 参见Freescale AN：Allan方差：陀螺仪的噪声分析
function [avar, tau, N, K, B] = ch_allan(omega, Fs, is_plot)

%[avar, tau] = allanvar(omega, 'octave', Fs);

t0 = 1/Fs;
theta = cumsum(omega, 1)*t0;  % cumsum 累积和

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


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*log(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));

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




if is_plot == true
    
    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
    axis equal
end



end

运行结果：

GYR: 零偏不稳定性                                                             0.000006rad/s                    或   1.323954deg/h 
GYR: 噪声密度(角度随机游走, ARW, Noise density)              0.000060(rad/s)/sqrt(Hz)     或  0.206710 deg/sqrt(h)
GYR: 角速率随机游走, bias variations ("random walks")       0.000008(rad/s)sqrt(Hz)       或  96.743587 deg/h/sqrt(h)
ACC: 零偏不稳定性                                                                                       0.000207m/s^(2)                       或   0.021148mg  或  21.148318ug
ACC: 噪声密度(速率随机游走,VRW, Noise Density, Rate Noise Density)          0.000560(m/s^(2))/sqrt(Hz)        或   0.033584 m/s/sqrt(hr)
ACC: 加速度随机游走，bias variations ("random walks")                               0.000036(m/s^(2)sqrt(Hz))