首先附一个介绍：NaveGo: an open-source MATLAB/GNU Octave toolbox for processing integrated navigation systems and performing inertial sensors analysis.
源码下载地址： NaveGo
编程语言：matlab
这个短小精悍的代码，主要包括以下几个部分：
（1）根据参考数据集和GPS datasheet，生成含有误差的GPS数据:
GPS 误差数据
% Garmin 5-18 Hz GPS error profile
GPS data structure:
t: Mx1 time vector (seconds).
lat: Mx1 latitude (radians).
lon: Mx1 longitude (radians).
h: Mx1 altitude (m).
vel: Mx3 NED velocities (m/s).
std: 1x3 position standard deviations (rad, rad, m).
stdm: 1x3 position standard deviations (m, m, m).
stdv: 1x3 velocity standard deviations (m/s).
larm: 3x1 lever arm (x-right, y-fwd, z-down) (m). 杆臂效应 右-前-下
freq: 1x1 sampling frequency (Hz).
主要是：以米为单位的latitude、longitude、高度误差设置；东北天三个速度误差设置、杆臂长度、频率等信息。
GPS仿真数据(真实数据+误差数据)
参考数据频率为100HZ，在此处主要是通过round函数进行降采样，完成！
% Downsampling GPS estimates from 1/dt Hz to freq Hz.
dt = mean(diff(ref.t));
freq = 1/dt;
dspl = round(freq / gps.freq); %四舍五入
dow = floor(m/dspl); %数据点 ；向负无穷取整
gps_r.t = ref.t (1:dspl:end, :);
gps_r.lat = ref.lat (1:dspl:end, :);
gps_r.lon = ref.lon (1:dspl:end, :);
gps_r.h = ref.h (1:dspl:end, :);
gps_r.vel = ref.vel (1:dspl:end, :);
gps_r.kn = dow;
gps_r.freq = round(1/mean(diff(gps_r.t)));（2）根据参考数据集和IMU datasheet，生成含有误差的IMU数据:
Reference.mat含有如下数据：
ref.mat contains the reference data structure from which inertial
sensors and GPS wil be simulated. It must contain the following fields:
t: Nx1 time vector (seconds).
lat: Nx1 latitude (radians).
lon: Nx1 longitude (radians).
h: Nx1 altitude (m).
vel: Nx3 NED velocities (m/s).
roll: Nx1 roll angles (radians).
pitch: Nx1 pitch angles (radians).
yaw: Nx1 yaw angle vector (radians).
kn: 1x1 number of elements of time vector.
DCMnb: Nx9 Direct Cosine Matrix nav-to-body. Each row contains
the elements of one matrix ordered by columns as
[a11 a21 a31 a12 a22 a32 a13 a23 a33].每行包含按照‘列’顺序排序的元素
freq: sampling frequency (Hz).
IMU Error Profile
% ADIS16405 IMU error profile
IMU data structure:
t: Ix1 time vector (seconds).
fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
wb: Ix3 turn rates vector in body frame XYZ (radians/s).
arw: 1x3 angle random walks (rad/s/root-Hz).
vrw: 1x3 velocity random walks (m/s^2/root-Hz).
gstd: 1x3 gyros standard deviations (radians/s).
astd: 1x3 accrs standard deviations (m/s^2).
gb_fix: 1x3 gyros static biases or turn-on biases (radians/s).常值零偏
ab_fix: 1x3 accrs static biases or turn-on biases (m/s^2).
gb_drift: 1x3 gyros dynamic biases or bias instabilities
(radians/s).零偏稳定性/零漂
ab_drift: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
gb_corr: 1x3 gyros correlation times (seconds).
ab_corr: 1x3 accrs correlation times (seconds).
gpsd : 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
apsd : 1x3 accrs dynamic biases PSD (m/s^2/root-Hz);
freq: 1x1 sampling frequency (Hz).
ini_align: 1x3 initial attitude at t(1).
ini_align_err: 1x3 initial attitude errors at t(1).
ref dataset will be used to simulate IMU sensors.
解析一下，源码中是怎么根据Datasheet 生成IMU误差的：
只考虑了三种误差：常值零偏、零偏稳定性(零漂/In-Run Bias Stability)、随机游走(RW)
其中生成噪声的代码段如下：
% Set static bias randomly from interval
a = -imu.gb_fix(1);
b = imu.gb_fix(1);
gb_fix = (b-a).*rand(3,1) + a;
% Random vectors
r1=randn(ref.kn,1);
r2=randn(ref.kn,1);
r3=randn(ref.kn,1);
r4=randn(ref.kn,1);
r5=randn(ref.kn,1);
r6=randn(ref.kn,1);
o=ones(ref.kn,1);
if (isinf(imu.gb_corr))
sigmc = imu.gb_drift;
gb_corr = [sigmc(1).*r4 sigmc(2).*r5 sigmc(3).*r6];
else
% Gyro correlation noise
gb_corr = zeros(ref.kn,3);
dt=mean(diff(ref.t));
alpha = exp(-dt./imu.gb_corr);
sigmc = imu.gb_drift .* sqrt(1 - alpha.^2);
gb_corr(1,:) = gb_fix' + [sigmc(1).*r4(1) sigmc(2).*r5(1) sigmc(3).*r6(1)];
for i=2:ref.kn
gb_corr (i,:) = alpha .* gb_corr (i-1,:) + [sigmc(1).*r4(i) sigmc(2).*r5(i) sigmc(3).*r6(i)];
end
end
wb = gyro_b + gyro_err_b + ...
[imu.gstd(1).*r1 imu.gstd(2).*r2 imu.gstd(3).*r3 ] + ...
[gb_fix(1).*o gb_fix(2).*o gb_fix(3).*o] + ...
gb_corr;
代码解析：以陀螺仪为例；
ARW为角度随机游走，其表达成为高斯白噪声序列：
imu_nav.gstd = imu_nav.arw ./ sqrt(dt); % rad/s/root-Hz -> rad/s
r1=randn(ref.kn,1);
imu.gstd(1).*r1
gb_fix为常值零偏，其每次开机都不同，但是开机之后为固定值；因此代码中将常值零偏设定为【-常值零偏 +常值零偏】之间的任意一个数；
In_Run Bias Stability为零偏稳定性，将其设为一阶马尔可夫过程，计算公式如下：
加速度噪声也假定为：速度随机游走、常值零偏、零偏稳定性；其代码配置方式和陀螺仪一样！
Gyroscope仿真数据生成(理想数据+误差数据)：
生成陀螺仪理想数据的数学公式如下：
% True gyro
if (isfield(ref, 'wb'))
gyro_b = ref.wb;
% Obtain gyros from DCM
else
gyro_raw = gyro_gen_delta(ref.DCMnb, diff(ref.t)); % 速率
gyro_raw = [gyro_raw; 0 0 0;];
gyro_b = sgolayfilt(gyro_raw, 10, 45);
end
% omega_en_n and omega_ie_n
gyro_err_b = zeros(ref.kn, 3);
for i = 1:ref.kn,
dcmnb = reshape(ref.DCMnb(i,:), 3, 3);
omega_ie_n = earthrate(ref.lat(i));
omega_en_n = transportrate(ref.lat(i), ref.vel(i,1), ref.vel(i,2), ref.h(i));
omega_in_b = dcmnb * (omega_en_n + omega_ie_n );
gyro_err_b(i,:) = ( omega_in_b )';
end加速度误差数据生成(理想数据+误差数据)：要简单的多，在此，不做表述！
（3）组合导航代码实现：
首先是进行时间“对齐”：
找到大于第一个gps数据时间点的 IMU数据时间；
IMU最后一个数据左边的最后一个GPS时间；
采用21误差状态，反馈的方式进行组合导航；
【Roll Pitch Yaw Vn Ve Vd Latitude Longitude Height Bias_gyro Bias_acc Dirft_gyro Drift_acc】
载体坐标系为：前右下；
导航坐标系为：北东地；
（4）plot；线性插值进行时间对齐
%Interpolate reference dataset
ref_1 = navego_interpolation (imu1_e, ref);
ref_2 = navego_interpolation (imu2_e, ref);
ref_g = navego_interpolation (gps, ref);采用 method = 'linear';方法进行插值；进行误差对比；
参考轨迹图：红圈处为开始点；
姿态误差图：
总结：在“飞机”起飞后，先做提升运动，然后向北飞行，100秒左右，拐弯大约90°，向东飞行；此时可以看到kalman滤波器能够迅速收敛，将误差估计到一个比较好的状态！
