本文介绍了一种利用EKF估计车辆IMU与车体、车轮安装角的方法。通过将里程计数据转换至不同坐标系，构建DR系统，结合SINS/GNSS数据进行后处理。实验结果显示，安装角估计快速收敛，标定因子误差稳定在1000ppm，验证了算法的有效性。
估计IMU和车辆之间的安装角度
前言
在SINS/NHC组合导航中，需要得到IMU和车辆之间的安装角，以及IMU和车轮（非转向轮）之间的杆臂。论文《Estimation of IMU Mounting Angles for Land Vehicular GNSS/INS Integrated System》给出了一种基于EKF的估计安装角的方法，有相应的论文和代码，代码链接如下：
Estimation of IMU Mounting Angles for Land Vehicular GNSS/INS Integrated System
其主要思路是把里程计速度（或里程增量）先从车辆坐标系转换到IMU坐标系，再从IMU坐标系转换到导航坐标系，构成一个DR系统，通过DR输出的位置和SINS/GNSS构成的后处理系统输出的位置相减，构成观测变量。对于没有里程计的车辆来说，代码中的思路是通过SINS/GNSS系统的PPK位置结果（导航系），通过转换矩阵转换到IMU坐标系，再转到车辆坐标系（初始IMU到车辆坐标系为单位矩阵）构成一个虚拟的里程计数据源。
论文翻译
安装角估计器包含一个DR产生器，这个DR产生器是基于SINS/GNSS系统产生的航向信息以及在车辆坐标系下产生的距离增量，这样一个DR产生器建立了一个这样的假设：车辆准确地满足车辆约束（NHC）。一旦不满足车辆约束，额外的误差就会进入DR系统。在仿真条件下，车辆被建模成一个点，并且绝对满足车辆约束。但是，在移动车载测量场景下，车辆约束有时候是不满足的，因为实际的车辆在转弯过程中可能会发生车轮的打滑，或者由于动力系统，减震系统会造成车辆的震动。对于火车这种场景，车辆约束通常是满足的，因为火车被严格限制在铁轨上运动，这可以解释为何在火车场景下，仿真和实际场景的结果是比较相近的。
另外，车后轮中心是最符合车辆约束的，因此，SINS/GNSS后处理系统的处理结果建立投影到车后轮中心，从公式22中可以看到，航向角误差会影响DR推算过程，并且影响安装角估计。
因此，为了保证安装角估计的比较准，有必要保证SINS/GNSS系统输出的航向角尽可能准。
论文公式推导
% 推导观测方程中和杆臂相关的量
syms oumiga_x oumiga_y oumiga_z % IMU和车辆之间安装角
syms lever_x lever_y lever_z % IMU和车辆之间的杆臂
syms b_x b_y b_z % 陀螺零偏
b = [b_x;b_y;b_z];
lever = [lever_x;lever_y;lever_z];
Cvb = [ 0,-oumiga_z, oumiga_y;
oumiga_z, 0,-oumiga_x;
-oumiga_y,oumiga_x, 0];
cross_lever = [ 0, -lever_z, lever_y;
lever_z, 0, -lever_x;
-lever_y, lever_x, 0];
cross_b = [ 0, -b_z, b_y;
b_z, 0, -b_x;
-b_y, b_x, 0];
data1 = Cvb * -cross_b * lever;
data2 = Cvb * cross_lever * b;
%data1
%lever_x*(b_y*oumiga_y + b_z*oumiga_z) - b_x*lever_y*oumiga_y - b_x*lever_z*oumiga_z
%lever_y*(b_x*oumiga_x + b_z*oumiga_z) - b_y*lever_x*oumiga_x - b_y*lever_z*oumiga_z
%lever_z*(b_x*oumiga_x + b_y*oumiga_y) - b_z*lever_x*oumiga_x - b_z*lever_y*oumiga_y
%data2
%b_y*lever_x*oumiga_y - b_x*(lever_y*oumiga_y + lever_z*oumiga_z) + b_z*lever_x*oumiga_z
%b_x*lever_y*oumiga_x - b_y*(lever_x*oumiga_x + lever_z*oumiga_z) + b_z*lever_y*oumiga_z
%b_x*lever_z*oumiga_x - b_z*(lever_x*oumiga_x + lever_y*oumiga_y) + b_y*lever_z*oumiga_y
% data1 = data2
% b_y*lever_x*oumiga_y - b_x*lever_y*oumiga_y + b_x*lever_z*oumiga_z + b_z*lever_x*oumiga_z
% b_x*lever_y*oumiga_x - b_y*lever_x*oumiga_x + b_y*lever_z*oumiga_z + b_z*lever_y*oumiga_z
% b_x*lever_z*oumiga_x - b_z*lever_x*oumiga_x + b_z*lever_y*oumiga_y + b_y*lever_z*oumiga_y
代码解读
1.main函数
function main()
close all;
% -------------------------------------------------------------------------
% This open-source program implementing the IMU mounting angle estimation
% KF may help the readers in understanding the algorithm designed in the
% manuscript -
% Qijin Chen, Xiaoji Niu, Jingnan Liu, "Estimation of
% IMU Mounting Angles for Land Vehicular GNSS/INS Integrated System"
%
% Please read the user manual for the definitions of the raw data format.
% -------------------------------------------------------------------------
% Author:
% Qijin Chen, GNSS Research Center, Wuhan University, China.;
% chenqijin@whu.edu.cn;
% Nov. 2019;
% -------------------------------------------------------------------------
% close all; clear; clc
% step 1: set configuration
cfg.fins = '.\data\ains_imu2.bin'; % raw GNSS/INS smoothing result; change to process different IMU data.
cfg.imutype = 'imu2'; % IMU type; 'imu1', 'imu2', 'imu3' for the three simulated IMUs
cfg.session = [800; 1200]; % time segment of the trajectory used as input to the KF estimator
% preprocess the GNSS/INS smoothing solution :1) derive traveled distance
% from GNSS/INS position; 2) resample GNSS/INS smoothing solution in
% distance domain.
% 0.1米
cfg.fds = 0.1; % distance interval for resample the raw GNSS/INS smoothing solution
data_ains = dataPreproc(cfg);
% Kalman filter tuning, P0, Q, R matrice.
cfg = paraTuning(cfg);
% step 2: KF estimator
nav = mas_ekf(data_ains, cfg);
% step 3: show results
cfg.plot_ma = 1;
cfg.plot_att_err = 1;
cfg.plot_odosf = 1;
plotResult(nav, cfg);
end
function cfg = paraTuning(cfg)
%% P0 matrix
% Uncertainty of the initial mounting angles(zeros).
cfg.ini_ma_std = [1; 2] *pi/180.0; % 1 deg for pitch-, 2 deg for heading-mounting angle, respectively.
% Uncertainty of the scale factor error of the distance measurement derived
% from GNSS/INS smoothing position.
% 标度因子
cfg.ini_odosf_std = 100 *1.0e-6; % 100 ppm
% Scale to enlarge the initial attitude uncertainty.
cfg.attstd_scale = 1.0; % 欧拉角不确定度的倍数
%% Q matrix
% ARW: angular random walk, i.e., white noise. of the gyro outputs
% 角度随机游走
switch cfg.imutype
case 'imu1'
cfg.q_arw = 0.0022 *pi/180.0/60; % deg/sqrt(h) -> rad/sqrt(s)
case 'imu2'
cfg.q_arw = 0.15 *pi/180.0/60;
case 'imu3'
cfg.q_arw = 0.1 *pi/180.0/60;
otherwise
cfg.q_arw = 0.1 *pi/180.0/60;
end
% covariance of the noise in modeling the pitch- and heading-mounting angles
cfg.q_ma_p = 0.001 *pi/180.0/60; % for pitch-mounting angle
cfg.q_ma_y = 0.001 *pi/180.0/60; % for heading-mounting angle
cfg.q_odosf = 1.0 *1.0e-6; % for scale factor error of the distance measurement
cfg.q_dt = 0.1; % average time interval between two DR epoch.
%% R matrix
cfg.kf.up_interval = 0.2; % the distance interval to use GNSS/INS position update.
% 位置置信度的倍数
cfg.kf.sf_R_pos = 3; % scale to enlarge the uncertainty of GNSS/INS position
end
2.dataPreproc函数
function data_ains = dataPreproc(cfg)
% -------------------------------------------------------------------------
%DATAPREPROC Preprocess the GNSS/INS smoothing solution: 1) calculate the
% traveled distance from GNSS/INS smoothing position; 2) resample
% GNSS/INS smoothing solution along distance. Prepare data for
% DR and mounting angle KF estimator.
% data_ains = dataPreproc(cfg)
%
%INPUTS:
% cfg = the configuration
%OUTPUTS:
% imu_ains = the data used as input to DR and mounting angle
% estimator. refer to table 2 for the format definition
% -------------------------------------------------------------------------
% Author:
% Qijin Chen, GNSS Research Center, Wuhan University, China.;
% chenqijin@whu.edu.cn;
% Nov. 2019;
% -------------------------------------------------------------------------
% read the GNSS/INS smoothing solution; refer to table 1 for the format
fid = fopen(cfg.fins, 'rb');
if fid == -1
disp('Cannot open the file!');
data_ains = [];
return;
end
% GNSS/INS data
data = fread(fid, [21,inf], 'double')';
fclose(fid); clear fid;
% the segment of trajectory used to estimate the mounting angles
n1 = find(data(:,1)>cfg.session(1), 1);
n2 = find(data(:,1)>cfg.session(2), 1);
data = data(n1:n2, :);
% calculate velocity in v-frame to help computing the distance increment
% data(idx,7:9):北东地速度（后处理结果）
v_v = zeros(size(data,1), 3);
for idx = 1:size(v_v,1)
cbn = eul2dcm(data(idx,10)*pi/180.0, data(idx,11)*pi/180.0, data(idx,12)*pi/180.0);
v_v(idx, :) = (cbn' * data(idx,7:9)')'; % 转到imu坐标系
end
dt = diff(data(:,1));
% 前，右，下（载体坐标系）
ds = v_v(1:end-1,1).*dt; %前进方向
vdist = [0; cumsum(ds(:))]; %里程累积值
ts_vel = [data(:,1), vdist];
[~, m, ~] = unique(ts_vel(:,2)); % 静止段去掉
ts_vel = ts_vel(m, :);
data = data(m,:);
% compute traveled distance from GNSS/INS smoothing position
%按照里程分割时间
s1 = min(ts_vel(:,2)):cfg.fds:max(ts_vel(:,2));
t1 = interp1(ts_vel(:,2), ts_vel(:,1), s1); % 插值
t1 = t1(:);
%ppk位置
pos = interp1(data(:,1), [data(:,5:6) data(:,4)], t1);
dpos = diff(pos);
ds = zeros(size(pos,1), 1);
for idx = 1:size(dpos,1)
ds(idx+1,1) = sqrt(dpos(idx,1)*dpos(idx,1)+dpos(idx,2)*dpos(idx,2)+dpos(idx,3)*dpos(idx,3));
end
ts_pos = [t1, cumsum(ds)]; % GPS里程累积
figure;
plot(data(:,3),data(:,2),'b-*');
% resample the GNSS/INS smoothing solution according to the distance
data(:,12) = yawSmooth(data(:,12));
data = interp1(data(:,1),data, ts_pos(:,1));
data(:,12) = data(:,12)-floor(data(:,12)/360)*360;%??
n = isnan(data(:,1));
data(n, :) = [];
% Unit conversion
data(:,2:3) = data(:,2:3)*pi/180; % rad
data(:,10:12) = data(:,10:12)*pi/180; % rad
data(:,19:21) = data(:,19:21)*pi/180; % rad
data_ains = [data, ts_pos(:,2)]; % GPS里程累积
end
function Az = yawSmooth(Az)
N = length(Az);
A_threshold = 180;
for i = 2:N
if Az(i) - Az(i-1) > A_threshold
Az(i:N) = Az(i:N) - 360;
elseif Az(i) - Az(i-1) < -A_threshold
Az(i:N) = Az(i:N) + 360;
end
end
end
3.mas_ekf函数
滤波主体函数
function nav = mas_ekf(data_ains, cfg)
% -------------------------------------------------------------------------
%MASEKF implementation of the mounting angle estimation Kalman filtering
% dcm = eul2dcm(roll, pitch, heading)
%INPUTS:
% 1. imu_ains = the data used as input to DR and mounting angle
% estimator. refer to table 2 for the format definition
% 2. cfg = the configuration
%OUTPUTS:
% 1. nav = Nx21 matrix, each column is defined as
% nav(i,1) = time in seconds
% nav(i,2) = distance in meters
% nav(i,3:5) = latitude(in radians),longitude(in radians), height
% (in meters);
% nav(i,6:7) = pitch-mounting angle(in radians),heading-mounting angle
% (in radians);
% nav(i,8:10) = estimated attitude errors in GNSS/INS smoothing roll,
% pitch and heading angle (in radians);
% nav(i,11) = scale factor error of the distance measurement.
% -------------------------------------------------------------------------
% Author:
% Qijin Chen, GNSS Research Center, Wuhan University, China.;
% chenqijin@whu.edu.cn;
% Nov. 2019;
% -------------------------------------------------------------------------
ds = [0; diff(data_ains(:,22))]; % distance increment between two epochs.
% initialize the dead reckoning positioning.
[nav, P, x, Q, G, Phi, datum] = initDR(data_ains, cfg);
cbv = eye(3); % initialize the mounting angles as zeros
% progress bar
num = size(data_ains,1);
if num > 100
fric = floor(num/100);
handle = waitbar(0, 'DR aided by GNSS/INS position!', 'Name', 'IMU Mounting Angle Estimator');
WaitTitle = 'Filtering...';
end
% DR/AINS position filtering
s_prev = data_ains(1,22);
for idx = 2:size(data_ains,1)
% show current progress
if num > 100
if mod(idx, fric) == 0
waitbar(idx/num, handle, WaitTitle);
end
end
meas = data_ains(idx,:);
cbn = eul2dcm(meas(10), meas(11), meas(12));
% feedback the estimated mounting angles to compensate the GNSS/INS attitude
% 车辆坐标系到导航系
cvn = cbn*cbv';
% distance increment resolved in navigation frame
% 导航系的里程增量
ds_n = cvn*[ds(idx); 0; 0];
% the radii of curvature along lines of constant longitude and
% latitude.
Rm = datum.a*(1-datum.e2)/((1-datum.e2 * sin(data_ains(idx,2))^2)^(3/2));
Rn = datum.a/sqrt(1-datum.e2 * sin(data_ains(idx,2))^2);
% DR position update
nav(idx, 3) = nav(idx-1,3) + ds_n(1)/(Rm + data_ains(idx,4));
nav(idx, 4) = nav(idx-1,4) + ds_n(2)/(Rn + data_ains(idx,4))/cos(data_ains(idx,2));
nav(idx, 5) = nav(idx-1,5) - ds_n(3);
% Set the state transition matrix PHI
M = [0 0; 0 -abs(ds(idx)); abs(ds(idx)) 0];
% 4-5：安装角
% 6-8：欧拉角
% 9：标度因子
% 公式推导
Phi(1:3,4:5) = -cvn*M;
Phi(1:3,6:8) = cp_form(ds_n);
Phi(1:3,9) = ds_n(:);
% Kalman filter prediction, time update of the state and P matrix
[x, P] = Kalman_predict(x, P, Phi, G, Q);
% Kalman filter measurement update
% The aided INS positioning solution is used as measurement update for
% the DR system
% up_interval:the distance interval to use GNSS/INS position update
if abs(data_ains(idx,22)-s_prev) > cfg.kf.up_interval
r_gps_e = blh2xyz(datum.e2, Rn, data_ains(idx,2:4)');
% nav(idx,3:5):DR推算位置
r_ins_e = blh2xyz(datum.e2, Rn, nav(idx,3:5)');
la_r = zeros(3,1);
C_en = cne(data_ains(idx,2), data_ains(idx,3))';
% 观测数据
z = C_en * (r_ins_e - r_gps_e) + la_r;
% Measurement equations
% Hm：观测转测矩阵
Hm = zeros(3,9);
Hm(1:3,1:3) = eye(3);
% 新息：inno
inno = z - Hm * x; % innovation
R = diag((cfg.kf.sf_R_pos*data_ains(idx,13:15)).^2);
[x, P, pdf] = Kalman_update(x, P, inno, Hm, R);
if pdf ~= 0
return
end
% position feedback
% 反馈了位置
d_lat = x(1)/(Rm + nav(idx,5));
d_lon = x(2)/(Rn + nav(idx,5))/cos(nav(idx,3));
d_theta = [d_lon * cos(nav(idx,3)); -d_lat; -d_lon * sin(nav(idx,3))];
% rv2quat：旋转变量转四元数
qn = rv2quat(-d_theta);
q_ne = qne(nav(idx,3), nav(idx,4));
q_ne = quatprod(q_ne, qn);
[nav(idx,3), nav(idx,4)] = qne2bl(q_ne);
nav(idx,5) = nav(idx,5) + x(3); % 高程直接反馈
% Update the mounting angle estimates
% 反馈了安装角
cvv = eul2dcm(0, x(4), x(5));
cbv = cvv*cbv; % 更新车辆坐标系到导航系
% set the state components as zeros after feedback
% 状态变量置0
x(1:5,1) = zeros(1,5);
end
[~, nav(idx,6), nav(idx,7)] = dcm2eul(cbv);
nav(idx,8:10) = x(6:8);
nav(idx,11) = x(9);
end
% close progress bar
if num > 100
close(handle);
end
end
% Navigation initialization
function [nav, P, x, Q, G, Phi, datum] = initDR(imu, cfg)
datum = earthModel();
% initialize the DR position with adied INS positioning solutions
nav = zeros(length(imu), 11);
nav(:,1:2) = [imu(:,1), imu(:,22)]; % time and distance index.
nav(1,3:5) = imu(1,2:4); % initialize the DR position from GNSS/INS smoothing position
% Initialize the state error covariance matrix, P0
% 9维：
% 1-3：位置
% 4-5：安装角
% 6-8：欧拉角
% 9：标度因子
P = zeros(9,9);
% 位置不确定度使用PPK的位置不确定度
P(1:3,1:3) = diag(1.0*(imu(1,13:15)).^2); % position !!!! 1.0 位置的不确定度
P(4:5,4:5) = diag(cfg.ini_ma_std.^2); % mounting angles 安装角度
P(6:8,6:8) = diag((cfg.attstd_scale*imu(1,19:21)).^2); % attitude uncertainty 欧拉角不确定度
P(9,9) = cfg.ini_odosf_std^2; % incremental distance scale factor error 标度因子
% state vector initialization
x = zeros(9,1);
% set system noise covariance matrix, Q, time-invariant
Q = diag([cfg.q_ma_p; % for pitch-mounting angle
cfg.q_ma_y; % for heading-mounting angle
cfg.q_arw; % 角度随机游走
cfg.q_arw;
cfg.q_arw;
cfg.q_odosf].^2)*cfg.q_dt; % for scale factor error of the distance measurement
% discrete-time system noise distribution matrix
% 噪声转换矩阵
% 安装角：4-5
% 欧拉角：6-8
% 标度因子：9
G = zeros(9,6);
G(4:9, 1:6) = eye(6);
% initilization of the discrete-time transition matrix
% 状态转移矩阵
Phi = eye(9);
end
% Kalman Filter prediction
function [x_pred, P_pred] = Kalman_predict(x, P, PHI, Gamma, Qw)
x_pred = PHI * x;
P_pred = PHI * P * PHI' + Gamma * Qw * Gamma';
end
% Kalman filter measurement update
function [x_up, P_up, pdf] = Kalman_update(x, P, inno, H, R)
PHt = P * H';
HPHt = H * PHt;
RHPHt = R + HPHt;
% chol？？
[U, pdf] = chol(RHPHt);
if pdf == 0 % positive definite
U = inv(U);
U = U * U';
K = PHt * U;
dx = K * inno;
x_up = x + dx; % 状态变量更新
IKH = eye(length(x)) - K * H;
P_up = IKH * P * IKH' + K * R * K'; % P矩阵更新
else
x_up = 0;
P_up = zeros(5);
end
end
4.plotResult函数
function plotResult(nav, cfg)
% -------------------------------------------------------------------------
%PLOTRESULT plot the results
% -------------------------------------------------------------------------
% Author:
% Qijin Chen, GNSS Research Center, Wuhan University, China.;
% chenqijin@whu.edu.cn;
% Nov. 2019;
% -------------------------------------------------------------------------
t0 = floor(nav(1,1)/100)*100;
%% Figure1, plot the mounting angle estimate
if cfg.plot_ma
figure,plot(nav(:,1)-t0, nav(:,6:7)*180/pi, 'linewidth', 2.0);
set(gca, 'fontsize', 12);
xlabel(['Time -' num2str(t0) ' / s'], 'fontsize', 12);
ylabel('deg', 'fontsize', 12);
legend({'\delta\theta', '\delta\psi'}, 'orientation', 'horizontal', 'Box', 'off', 'fontsize', 13);
title('Mounting Angle Estimate', 'fontsize', 12, 'color', 'k');
ylim([0 4]);
end
%% figure2, plot the estimated errors in the GNSS/INS smoothing attitude
if cfg.plot_att_err
figure,
f1 = subplot(311); plot(nav(:,1)-t0, nav(:,8)*180/pi, 'color', [0.3 0.3 0.3]);
set(gca, 'fontsize', 12);
ylabel('\phi / deg', 'fontsize', 12, 'color', 'r');
title('Attitude Error Estimate', 'fontsize', 12, 'color', 'k');
f2 = subplot(312); plot(nav(:,1)-t0, nav(:,9)*180/pi, 'color', [0.3 0.3 0.3]);
set(gca, 'fontsize', 12);
ylabel('\theta / deg', 'fontsize', 12, 'color', 'r');
f3 = subplot(313); plot(nav(:,1)-t0, nav(:,10)*180/pi, 'color', [0.3 0.3 0.3]);
set(gca, 'fontsize', 12);
ylabel('\psi / deg', 'fontsize', 12, 'color', 'r');
xlabel(['Time -' num2str(t0) ' / s'], 'fontsize', 12);
linkaxes([f1 f2 f3], 'x');
end
%% figure3, plot the estimated scale factor of the distance measurements.
if cfg.plot_odosf
figure,plot(nav(:,1)-t0, nav(:,11)*1.0e6, 'color', [0.3 0.3 0.3]);
set(gca, 'fontsize', 12);
xlabel(['Time -' num2str(t0) ' / s'], 'fontsize', 12);
ylabel('PPM', 'fontsize', 12);
title('Odometer Scale Factor Error Estimate', 'fontsize', 12, 'color', 'k');
end
end
代码修改
开源的代码的中的坐标系定义如下：
IMU坐标系：前，右，下（X，Y、Z坐标系）
车体坐标系：前，右，下（X，Y、Z坐标系）
欧拉角：roll,pitch,yaw
速度：北，东，地
位置：纬度，经度
我习惯的坐标系定义如下：
IMU坐标系：右，前，上（X，Y、Z坐标系）
车体坐标系：右，前，上（X，Y、Z坐标系）
欧拉角：pitch,roll,yaw
速度：东，北，天
位置：经度，纬度
所以对代码做了一定的修改，修改之后的代码如下：
1.main函数
function main()
close all;
% -------------------------------------------------------------------------
% This open-source program implementing the IMU mounting angle estimation
% KF may help the readers in understanding the algorithm designed in the
% manuscript -
% Qijin Chen, Xiaoji Niu, Jingnan Liu, "Estimation of
% IMU Mounting Angles for Land Vehicular GNSS/INS Integrated System"
%
% Please read the user manual for the definitions of the raw data format.
% -------------------------------------------------------------------------
% Author:
% Qijin Chen, GNSS Research Center, Wuhan University, China.;
% chenqijin@whu.edu.cn;
% Nov. 2019;
% -------------------------------------------------------------------------
% close all; clear; clc
% step 1: set configuration
%
%cfg.fins = '.\data\20211203T093011_5X_ie_psins_test_nhc_simVRS.txt'; % raw GNSS/INS smoothing result; change to process different IMU data.
%cfg.fins = '.\data\20211203T093011_5X_ie_psins_VRS.txt'; % raw GNSS/INS smoothing result; change to process different IMU data.
cfg.fins = '.\data\20211125T152144_ie_psins_jizhan.txt'; % raw GNSS/INS smoothing result; change to process different IMU data.
cfg.imutype = 'imu4'; % IMU type; 'imu1', 'imu2', 'imu3' for the three simulated IMUs
%cfg.fins = '.\data\20211125T152144_ie_psins_jizhan.txt'; % raw GNSS/INS smoothing result; change to process different IMU data.
%cfg.imutype = 'imu5'; % IMU type; 'imu1', 'imu2', 'imu3' for the three simulated IMUs
cfg.session = [500; 680]; % time segment of the trajectory used as input to the KF estimator
% preprocess the GNSS/INS smoothing solution :1) derive traveled distance
% from GNSS/INS position; 2) resample GNSS/INS smoothing solution in
% distance domain.
% 0.1米
cfg.fds = 0.1; % distance interval for resample the raw GNSS/INS smoothing solution
data_ains = dataPreproc(cfg);
% Kalman filter tuning, P0, Q, R matrice.
cfg = paraTuning(cfg);
% step 2: KF estimator
nav = mas_ekf(data_ains, cfg);
% step 3: show results
cfg.plot_ma = 1;
cfg.plot_att_err = 1;
cfg.plot_odosf = 1;
plotResult(nav, cfg);
end
function cfg = paraTuning(cfg)
%% P0 matrix
% Uncertainty of the initial mounting angles(zeros).
cfg.ini_ma_std = [1; 2] *pi/180.0; % 1 deg for pitch-, 2 deg for heading-mounting angle, respectively.
% Uncertainty of the scale factor error of the distance measurement derived
% from GNSS/INS smoothing position.
% 标度因子
cfg.ini_odosf_std = 100 *1.0e-6; % 100 ppm
% Scale to enlarge the initial attitude uncertainty.
cfg.attstd_scale = 3.0; % 欧拉角不确定度的倍数
%% Q matrix
% ARW: angular random walk, i.e., white noise. of the gyro outputs
% 角度随机游走
switch cfg.imutype
case 'imu1'
cfg.q_arw = 0.0022 *pi/180.0/60; % deg/sqrt(h) -> rad/sqrt(s)
case 'imu2'
cfg.q_arw = 0.15 *pi/180.0/60;
case 'imu3'
cfg.q_arw = 0.1 *pi/180.0/60;
case 'imu4'
cfg.q_arw = 0.1 *pi/180.0/60; % FSAS
case 'imu5'
cfg.q_arw = 0.5 *pi/180.0/60; % CPT7
otherwise
cfg.q_arw = 0.1 *pi/180.0/60;
end
% covariance of the noise in modeling the pitch- and heading-mounting angles
cfg.q_ma_p = 0.01 *pi/180.0/60; % for pitch-mounting angle
cfg.q_ma_y = 0.01 *pi/180.0/60; % for heading-mounting angle
cfg.q_odosf = 1.0 *1.0e-6; % for scale factor error of the distance measurement
cfg.q_dt = 0.05; % average time interval between two DR epoch.
%% R matrix
cfg.kf.up_interval = 0.2; % the distance interval to use GNSS/INS position update.
% 位置置信度的倍数
cfg.kf.sf_R_pos = 3; % scale to enlarge the uncertainty of GNSS/INS position
end
2. mas_ekf函数
% 状态变量：
% 9维：
% 1-3：位置
% 4-5：安装角
% 6-8：欧拉角
% 9：标度因子
% data_ains(1,):时间戳
% data_ains(1,2:4):经度,纬度,高程（后处理结果）
% data_ains(1,5:6): 东，北位置
% data_ains(1,7:9):东北天速度（后处理结果）
% data_ains(1,10:12):pitch,roll,yaw（后处理结果）
% data_ains(1,13:15)：位置置信度 (经度，纬度，高程)
% data_ains(1,16:18)：速度置信度（东，北，天速度）
% data_ains(1,19:21)：欧拉角置信度（pitch,roll,yaw）
% data_ains(1,22)：GPS累积里程
function nav = mas_ekf(data_ains, cfg)
% -------------------------------------------------------------------------
%MASEKF implementation of the mounting angle estimation Kalman filtering
% dcm = eul2dcm(roll, pitch, heading)
%INPUTS:
% 1. imu_ains = the data used as input to DR and mounting angle
% estimator. refer to table 2 for the format definition
% 2. cfg = the configuration
%OUTPUTS:
% 1. nav = Nx21 matrix, each column is defined as
% nav(i,1) = time in seconds
% nav(i,2) = distance in meters
% nav(i,3:5) = longitude(in radians)，latitude(in radians), height
% (in meters);
% nav(i,6:7) = pitch-mounting angle(in radians),heading-mounting angle
% (in radians);
% nav(i,8:10) = estimated attitude errors in GNSS/INS smoothing pitch,
% roll and heading angle (in radians);
% nav(i,11) = scale factor error of the distance measurement.
% -------------------------------------------------------------------------
% Author:
% Qijin Chen, GNSS Research Center, Wuhan University, China.;
% chenqijin@whu.edu.cn;
% Nov. 2019;
% -------------------------------------------------------------------------
ds = [0; diff(data_ains(:,22))]; % distance increment between two epochs.
figure;
plot(ds,'b-*');
title('ds')
% initialize the dead reckoning positioning.
[nav, P, x, Q, G, Phi, datum] = initDR(data_ains, cfg);
cbv = eye(3); % initialize the mounting angles as zeros
% progress bar
num = size(data_ains,1);
if num > 100
fric = floor(num/100);
handle = waitbar(0, 'DR aided by GNSS/INS position!', 'Name', 'IMU Mounting Angle Estimator');
WaitTitle = 'Filtering...';
end
% DR/AINS position filtering
s_prev = data_ains(1,22);
x_pos_sum = zeros(size(data_ains,1) - 1,3);
x_post_pos_sum = zeros(size(data_ains,1) - 1,3);
inno_sum = zeros(size(data_ains,1) - 1,3);
for idx = 2:size(data_ains,1)
% show current progress
if num > 100
if mod(idx, fric) == 0
waitbar(idx/num, handle, WaitTitle);
end
end
% data_ains(idx,10:12):姿态角,方位角（后处理结果）
meas = data_ains(idx,:);
cbn = a2mat([meas(10),meas(11),meas(12)]);
% feedback the estimated mounting angles to compensate the GNSS/INS attitude
% 车辆坐标系到导航系
% cbn:IMU坐标系到导航坐标系
% cvn = cbn * cvb
cvn = cbn*cbv';
% distance increment resolved in navigation frame
% 导航系的里程增量
% 注意:不是速度
% 车辆坐标系，隐含车辆约束(NHC)
% 这里坐标系：右，前，上
% 【车辆坐标系】里程增量转【导航系】里程增量
% sim_dr_ratio:模拟里程计标度因子，查看估计出来的标度因子和实际的相不相符
sim_dr_ratio = 0.999;
ds_n = cvn*[0;ds(idx) * sim_dr_ratio;0];
% the radii of curvature along lines of constant longitude and
% latitude.
% 全局变量
Rm = datum.a*(1-datum.e2)/((1-datum.e2 * sin(data_ains(idx,3))^2)^(3/2));
Rn = datum.a/sqrt(1-datum.e2 * sin(data_ains(idx,3))^2);
% DR position update
% 导航坐标系
% DR过程
% 弧度
nav(idx, 3) = nav(idx-1,3) + ds_n(1)/(Rn + data_ains(idx,4))/cos(data_ains(idx,3)); % 经度
nav(idx, 4) = nav(idx-1,4) + ds_n(2)/(Rm + data_ains(idx,4)); % 纬度
nav(idx, 5) = nav(idx-1,5) + ds_n(3); % 高程
% Set the state transition matrix PHI
M = [0 abs(ds(idx)); 0 0; -abs(ds(idx)) 0]; %更改！！
% 4-5：安装角
% 6-8：欧拉角
% 9：标度因子
% 公式推导
% Phi：状态转移矩阵
% 参考论文公式（27）
Phi(1:3,4:5) = -cvn*M; % 和IMU安装角相关
% cp_form:叉乘形式
Phi(1:3,6:8) = cp_form(ds_n); % 和失准角相关
Phi(1:3,9) = ds_n(:); % 和标度因子相关，正负号不影响结果
% Kalman filter prediction, time update of the state and P matrix
[x, P] = Kalman_predict(x, P, Phi, G, Q);
x_pos_sum(idx - 1,:) = x(1:3)';
% Kalman filter measurement update
% The aided INS positioning solution is used as measurement update for
% the DR system
% up_interval:the distance interval to use GNSS/INS position update
% cfg.kf.up_interval = 0.2; the distance interval to use GNSS/INS position update.
% data_ains(idx,22):累加值
if abs(data_ains(idx,22)-s_prev) > cfg.kf.up_interval
z = nav(idx,3:5)' - data_ains(idx,2:4)';
z(1) = z(1) * Rn * cos(data_ains(idx,3));
z(2) = z(2) * Rm;
% Measurement equations
% Hm：观测转换矩阵
% Hm(1:3,1:3) = eye(3);
% 只有位置观测
Hm = zeros(3,9);
Hm(1:3,1:3) = eye(3);
% 新息：inno
% 只有位置观测
inno = z - Hm * x; % innovation
inno_sum(idx -1,:) = inno';
% 观测噪声
R = diag((cfg.kf.sf_R_pos*data_ains(idx,13:15)).^2);
[x, P, pdf] = Kalman_update(x, P, inno, Hm, R);
x_post_pos_sum(idx - 1,:) = x(1:3)';
if pdf ~= 0
return
end
% position feedback
% 反馈了位置
% d_lat：单位：弧度
% d_lon：单位：弧度
d_lon = x(1)/(Rn + nav(idx,5))/cos(nav(idx,4));
d_lat = x(2)/(Rm + nav(idx,5));
nav(idx,3) = nav(idx,3) - d_lon;
nav(idx,4) = nav(idx,4) - d_lat;
nav(idx,5) = nav(idx,5) - x(3); % 高程直接反馈
% Update the mounting angle estimates
% 反馈了安装角
%cvv = eul2dcm(0, x(4), x(5));
cvv = a2mat([x(4),0, x(5)]);
cbv = cvv*cbv; % 更新车辆坐标系到导航系
% set the state components as zeros after feedback
% 状态变量置0
x(1:5,1) = zeros(1,5);
end
%nav(i,6:7) = pitch-mounting angle(in radians),heading-mounting angle(in radians)
%[~, nav(idx,6), nav(idx,7)] = dcm2eul(cbv);
att = m2att(cbv);
nav(idx,6) = att(1);
nav(idx,7) = att(3);
%nav(i,8:10) = estimated attitude errors in GNSS/INS smoothing roll
nav(idx,8:10) = x(6:8);
%scale factor error of the distance measurement.
nav(idx,11) = x(9);
end
figure;
plot(nav(:,3),nav(:,4),'b-*');
hold on;
plot(data_ains(:,2),data_ains(:,3),'r-*');
hold on;
plot(data_ains(1,2),data_ains(1,3),'k*');
axis equal;
title('dr-gps-compare')
legend('dr-pos','gps-pos')
% close progress bar
if num > 100
close(handle);
end
end
% Navigation initialization
function [nav, P, x, Q, G, Phi, datum] = initDR(imu, cfg)
datum = earthModel();
% initialize the DR position with adied INS positioning solutions
% 9维：
% 1-3：位置:经度,纬度,高程
% 4-5：安装角
% 6-8：欧拉角
% 9：标度因子
nav = zeros(length(imu), 11);
nav(:,1:2) = [imu(:,1), imu(:,22)]; % time and distance index.
nav(1,3:5) = imu(1,2:4); % initialize the DR position from GNSS/INS smoothing position
% Initialize the state error covariance matrix, P0
% 9维：
% 1-3：位置
% 4-5：安装角
% 6-8：欧拉角
% 9：标度因子
P = zeros(9,9);
% 位置不确定度使用PPK的位置不确定度
P(1:3,1:3) = diag(1.0*(imu(1,13:15)).^2); % position !!!! 1.0 位置的不确定度
P(4:5,4:5) = diag(cfg.ini_ma_std.^2); % mounting angles 安装角度
P(6:8,6:8) = diag((cfg.attstd_scale*imu(1,19:21)).^2); % attitude uncertainty 欧拉角不确定度
P(9,9) = cfg.ini_odosf_std^2; % incremental distance scale factor error 标度因子
% state vector initialization
x = zeros(9,1);
% set system noise covariance matrix, Q, time-invariant
Q = diag([cfg.q_ma_p; % for pitch-mounting angle
cfg.q_ma_y; % for heading-mounting angle
cfg.q_arw; % 角度随机游走
cfg.q_arw; % 角度随机游走
cfg.q_arw; % 角度随机游走
cfg.q_odosf].^2)*cfg.q_dt; % for scale factor error of the distance measurement
% discrete-time system noise distribution matrix
% 噪声转换矩阵
% 安装角：4-5
% 欧拉角：6-8
% 标度因子：9
G = zeros(9,6);
G(4:9, 1:6) = eye(6);
% initilization of the discrete-time transition matrix
% 状态转移矩阵
Phi = eye(9);
end
% Kalman Filter prediction
function [x_pred, P_pred] = Kalman_predict(x, P, PHI, Gamma, Qw)
x_pred = PHI * x;
P_pred = PHI * P * PHI' + Gamma * Qw * Gamma';
end
% Kalman filter measurement update
function [x_up, P_up, pdf] = Kalman_update(x, P, inno, H, R)
PHt = P * H';
HPHt = H * PHt;
RHPHt = R + HPHt;
[U, pdf] = chol(RHPHt); % Cholesky分解
if pdf == 0 % positive definite 正定
U = inv(U);
U = U * U';
K = PHt * U;
dx = K * inno;
x_up = x + dx; % 状态变量更新
IKH = eye(length(x)) - K * H;
P_up = IKH * P * IKH' + K * R * K'; % P矩阵更新
else
x_up = 0;
P_up = zeros(5);
end
end
3.dataPrepro函数
function data_ains = dataPreproc(cfg)
% -------------------------------------------------------------------------
%DATAPREPROC Preprocess the GNSS/INS smoothing solution: 1) calculate the
% traveled distance from GNSS/INS smoothing position; 2) resample
% GNSS/INS smoothing solution along distance. Prepare data for
% DR and mounting angle KF estimator.
% data_ains = dataPreproc(cfg)
%
%INPUTS:
% cfg = the configuration
%OUTPUTS:
% imu_ains = the data used as input to DR and mounting angle
% estimator. refer to table 2 for the format definition
% -------------------------------------------------------------------------
% Author:
% Qijin Chen, GNSS Research Center, Wuhan University, China.;
% chenqijin@whu.edu.cn;
% Nov. 2019;
% -------------------------------------------------------------------------
% read the GNSS/INS smoothing solution; refer to table 1 for the format
% data(1,):时间戳
% data(1,2:4):经度,纬度,高程（后处理结果）
% data(1,5:6): 东，北位置
% data(1,7:9):东北天速度（后处理结果）
% data(1,10:12):pitch,roll,yaw（后处理结果）
% data(1,13:15)：位置置信度 (经度，纬度，高程)
% data(1,16:18)：速度置信度（东，北，天速度）
% data(1,19:21)：欧拉角置信度（pitch,roll,yaw）
fid = fopen(cfg.fins, 'rb');
if fid == -1
disp('Cannot open the file!');
data_ains = [];
return;
end
% GNSS/INS data
%data = fread(fid, [21,inf], 'double')';
data = load(cfg.fins);
fclose(fid); clear fid;
% the segment of trajectory used to estimate the mounting angles
% data(:,1)：时间戳
% cfg.session(1)：开始时间
% cfg.session(2)：结束时间
n1 = find(data(:,1)>cfg.session(1), 1);
n2 = find(data(:,1)>cfg.session(2), 1);
data = data(n1:n2, :);
%位置： 经度，纬度，高程
lat = data(:,2);
lon = data(:,3);
data(:,2) = lon;
data(:,3) = lat;
% 北东地速度转换为东北天速度
n_vel = data(:,7);
e_vel = data(:,8);
d_vel = data(:,9);
data(:,7) = e_vel;
data(:,8) = n_vel;
data(:,9) = -d_vel;
% pitch和roll交换
roll = data(:,10);
pitch = data(:,11);
data(:,10) = pitch;
data(:,11) = roll;
sd_lat = data(:,13);
sd_lon = data(:,14);
data(:,13) = sd_lon;
data(:,14) = sd_lat;
sd_vel_n = data(:,16);
sd_vel_e = data(:,17);
data(:,16) = sd_vel_e;
data(:,17) = sd_vel_n;
sd_roll = data(:,19);
sd_pitch = data(:,20);
data(:,19) = sd_pitch;
data(:,20) = sd_roll;
% calculate velocity in v-frame to help computing the distance increment
v_v = zeros(size(data,1), 3);
for idx = 1:size(v_v,1)
cbn = a2mat([data(idx,10)*pi/180.0, data(idx,11)*pi/180.0, data(idx,12)*pi/180.0]);
v_v(idx, :) = (cbn' * data(idx,7:9)')'; % 转到imu坐标系
end
dt = diff(data(:,1));
% 右，前，上（载体坐标系）
ds = v_v(1:end-1,2).*dt;
vdist = [0; cumsum(ds(:))]; %里程累积值
ts_vel = [data(:,1), vdist];
% 静止或者低速条件下，不满足车辆约束
[~, m, ~] = unique(ts_vel(:,2)); % 静止段去掉
ts_vel = ts_vel(m, :);
data = data(m,:);
% compute traveled distance from GNSS/INS smoothing position
%按照里程分割时间
s1 = min(ts_vel(:,2)):cfg.fds:max(ts_vel(:,2));
t1 = interp1(ts_vel(:,2), ts_vel(:,1), s1); % 插值 等时间段
t1 = t1(:)
%ppk位置
pos = interp1(data(:,1), [data(:,5:6) data(:,4)], t1);
dpos = diff(pos);
ds = zeros(size(pos,1), 1);
for idx = 1:size(dpos,1)
ds(idx+1,1) = sqrt(dpos(idx,1)*dpos(idx,1)+dpos(idx,2)*dpos(idx,2)+dpos(idx,3)*dpos(idx,3));
end
ts_pos = [t1, cumsum(ds)]; % GPS里程累积
% resample the GNSS/INS smoothing solution according to the distance
% data(idx,10:12):姿态角,方位角（后处理结果）
data = interp1(data(:,1),data, ts_pos(:,1));
n = isnan(data(:,1));
data(n, :) = [];
% Unit conversion
data(:,2:3) = data(:,2:3)*pi/180; % rad 位置
data(:,10:12) = data(:,10:12)*pi/180; % rad 欧拉角
data(:,19:21) = data(:,19:21)*pi/180; % rad 欧拉角置信度
data_ains = [data, ts_pos(:,2)]; % GPS里程累积
end
% 设定范围：-180->180
function Az = yawSmooth(Az)
N = length(Az);
A_threshold = 180;
for i = 2:N
if Az(i) - Az(i-1) > A_threshold
Az(i:N) = Az(i:N) - 360;
elseif Az(i) - Az(i-1) < -A_threshold
Az(i:N) = Az(i:N) + 360;
end
end
end
4.plotResult函数
function plotResult(nav, cfg)
% -------------------------------------------------------------------------
%PLOTRESULT plot the results
% -------------------------------------------------------------------------
% Author:
% Qijin Chen, GNSS Research Center, Wuhan University, China.;
% chenqijin@whu.edu.cn;
% Nov. 2019;
% -------------------------------------------------------------------------
t0 = floor(nav(1,1)/100)*100;
%% Figure1, plot the mounting angle estimate
% nav(i,6:7) = pitch-mounting angle(in radians),heading-mounting angle
if cfg.plot_ma
figure,plot(nav(:,1)-t0, nav(:,6:7)*180/pi, 'linewidth', 2.0);
set(gca, 'fontsize', 12);
xlabel(['Time -' num2str(t0) ' / s'], 'fontsize', 12);
ylabel('deg', 'fontsize', 12);
legend({'\delta\theta', '\delta\psi'}, 'orientation', 'horizontal', 'Box', 'off', 'fontsize', 13);
title('Mounting Angle Estimate', 'fontsize', 12, 'color', 'k');
ylim([-2 4]);
end
%% figure2, plot the estimated errors in the GNSS/INS smoothing attitude
if cfg.plot_att_err
figure,
f1 = subplot(311); plot(nav(:,1)-t0, nav(:,8)*180/pi, 'b-*');
set(gca, 'fontsize', 12);
ylabel('\phi / deg', 'fontsize', 12, 'color', 'r');
title('Attitude Error Estimate', 'fontsize', 12, 'color', 'k');
f2 = subplot(312); plot(nav(:,1)-t0, nav(:,9)*180/pi, 'b-*');
set(gca, 'fontsize', 12);
ylabel('\theta / deg', 'fontsize', 12, 'color', 'r');
f3 = subplot(313); plot(nav(:,1)-t0, nav(:,10)*180/pi, 'b-*');
set(gca, 'fontsize', 12);
ylabel('\psi / deg', 'fontsize', 12, 'color', 'r');
xlabel(['Time -' num2str(t0) ' / s'], 'fontsize', 12);
linkaxes([f1 f2 f3], 'x');
end
%% figure3, plot the estimated scale factor of the distance measurements.
if cfg.plot_odosf
figure,plot(nav(:,1)-t0, nav(:,11)*1.0e6, 'b-*');
set(gca, 'fontsize', 12);
xlabel(['Time -' num2str(t0) ' / s'], 'fontsize', 12);
ylabel('PPM', 'fontsize', 12);
title('Odometer Scale Factor Error Estimate', 'fontsize', 12, 'color', 'k');
end
end
测试结果
1.里程计标定因子估计如下，代码中我们用GNSS数据模拟里程计，并且人为加入1000ppm的标度因子误差，从下图中可以看到，标度因子最终收敛到1000ppm附近。
2.失准角估计如下，可以看到航向角失准角在0.1deg左右。
3.估计的安装角如下，可以看到，收敛时间比较短，大约10几秒就可以实现收敛，收敛之后的平滑性也不错。
4.本身安装角和标度因子的估计过程也是一个D过程，DR如下，经过补偿之后的DR和GNSS位置差异较小，也就是标定任务的残差。
参考文献
1.Estimation of IMU Mounting Angles for Land Vehicular GNSS/INS Integrated System
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
