这里写自定义目录标题
前言
在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到车辆坐标系为单位矩阵）构成一个虚拟的里程计数据源。
论文公式推导
…
代码解读
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
参考文献
1.Estimation of IMU Mounting Angles for Land Vehicular GNSS/INS Integrated System
