文章内容仅限学习交流~
1、项目简介
所开发的 NaviSysPro 是一个用于 INS/GNSS 组合导航的工具箱，包含纯惯性导航、组合导航、传递对准、抗差滤波、车辆约束等功能模块。该工具箱旨在为导航和传感器集成提供高效的解决方案，适用于车辆、无人机和其他移动平台。
2、工具箱使用说明
-
本工具箱基于严恭敏老师的 PSINS 工具箱开发，使用前请先安装 PSINS 工具箱。
PSINS 相关资料请参考严老师网站：www.psins.org.cn/ -
使用时请确保将所有文件放在同一目录下，以防缺少必要的函数或模块。
3、所开发的函数列表及该函数的描述
| 函数名称 | 描述 | 参考文献 |
|---|---|---|
| mAccCaliDescent | 基于加速度计矢量模相等的标定方法，使用代价函数下降的搜索方法 | [1] |
| mAccCaliHxJx1 | 计算加速度计模型的雅各比和海森矩阵 | |
| mAcce6PosCalibration | 基于六位置修正的加速度计标定法 | [2] |
| GA算法框架（文件夹内所有函数） | 遗传算法框架，可用于非线性最优规划 | |
| ARkf | 基于马氏距离的抗差卡尔曼滤波算法 | [4],[9] |
| Adaptivekf156 | 序贯量测+方差限制算法，15x6维，估计零偏 | |
| Adaptivekf186 | 序贯量测+方差限制算法，18x6维，估计零偏和杆臂 | |
| Adaptivekf196 | 序贯量测+方差限制算法，18x6维，估计零偏、杆臂和同步时间 | |
| BayesianFilter | 受污染数据的贝叶斯滤波算法 | [5] |
| MRkf | 基于马氏距离的多因子修正抗差算法 | [3] |
| RobustKf | 多因子自适应抗差卡尔曼滤波算法 | |
| SH_KF156 | Sage-Husa自适应卡尔曼滤波（序贯量测） | |
| SlideWindow_Rkf | 滑动窗马氏距离检测滤波算法 | |
| kf | 标准卡尔曼滤波算法 | |
| kf0 | 精确时间融合的标准卡尔曼滤波算法 | |
| kf_phi | 增加航向观测的卡尔曼滤波 | |
| Virtual_Lever_Arm | 虚拟杆臂估计方法 | [6] |
| kfCalibrated_Xu | 陀螺仪的两步修正法标定 | [7] |
| mGyro6PosCalibration | 六位置法陀螺仪标定 | |
| mfunRef43 | 系统级标定法 | [8] |
| mgenRot | 生成旋转矩阵 | |
| NHC | 车辆不完整约束处理 | |
| UKF_constrait | 用于车辆不完整约束的UKF算法 | |
| Vehicle_Constraint | 使用高程+航向+不完整约束的车辆导航修正算法 | |
| AttAddKmtcCons | GNSS失锁解决方案：姿态匹配+车体运动学速度匹配，20维传递对准算法 | |
| Att_Vel_AddKmtcCons | GNSS失锁解决方案：姿态匹配+速度匹配，23维传递对准算法 | |
| Att_Vel_AddKmtcCons1 | GNSS失锁方案，增加正常段使用车辆约束 | |
| P_AdaptiveKf_NHC_PR | 17维位置组合卡尔曼滤波算法，适用于GNSS缺失时使用运动学约束 | |
| VmP_NHC | 车体速度辅助+位置，失锁时使用NHC | |
| VmP_NHC_RTS | 车体速度辅助+位置，使用NHC/RTS修正 | [11] |
| VnP_NHC | 导航速度辅助+位置，失锁时使用NHC | |
| ZeroVelocity_Kf | 零速修正卡尔曼滤波 | |
| ins_inpure | 纯惯性导航算法 | |
| RTS | RTS平滑算法核心代码 | |
| smooth_RTS | RTS平滑，平滑区间与外部观测频率一致 | |
| smooth_RTS_all | RTS平滑，使用全段数据进行平滑 | |
| smooth_TKF | 双向平滑算法 | [9] |
| Aligni0 | 粗对准算法 | |
| Arw_Vrw2std | ARW、VRW 与噪声标准差的转换 | |
| EarthParameter | 地球参数计算 | |
| Expand_axis_fill_figure | 去除图像边界空白区域 | |
| KF_Phi | 组合导航离散线性模型 | |
| Myavp2imu | 根据AVP数据反向生成IMU数据 | |
| WaveDenoise | IMU小波去噪处理 | |
| avp_update | 导航数据更新 | |
| feed_back_correct | 反馈校正方法 | |
| fplot | 导航误差计算并作图 | |
| genImuAsb | 生成非正交变换矩阵，转换理想b系到传感器系 | |
| globalParameter | 地球参数全局变量 | |
| mToolLatLonErrorMeters | 弧度转化为米 | |
| mxGetGravity | 计算重力值 | |
| reverseEarthParameter | 反向滤波更新地球参数值 | |
| rmse | 计算均方根误差（RMSE） | |
| slideVarStd | 滑动窗口计算数据方差、标准差和均值 | |
| xkplot | 滤波估计值和协方差绘图 | |
| SINS153Vel_TransferAlignment | 速度匹配的15维传递对准算法，仅估计杆臂 | |
| SINS186Att_Vel_TransferAlignment | 姿态和速度匹配的18维传递对准算法，估计安装角和杆臂 | |
| VPmaster | 主惯导速度位置匹配算法 | |
| UKF156 | 15维组合导航的UKF算法 | |
| UKFParameter | UKF参数设置 | |
| state_function | UKF非线性模型 | |
| ukf_filter | UKF滤波算法 | |
| utChange | UT变换 | |
| Wavelet_Transform（文件夹） | 小波变换算法 | |
| SimuAcc | 加速度计仿真数据生成器 | |
| TrjSim_INS | 轨迹仿真器 |
4、代码展示
整个工具箱包含内容如下：
展示其中的某一个函数 SINS186Att_Vel_TransferAlignment ，如下：
function res = SINS186Att_Vel_TransferAlignment(imu, mavp, imuerr, davp,avp0)
%% -----------Introduction------------
%姿态速度匹配18维传递对准算法 可以估计主子安装角，杆臂
%input:
%-------imu : 传感器数据N*7 单位：rad/s m/s^2
%-------davp : 用于设置Kalman P阵 18*1
%-------imuerr : 用于设置Kalman Q阵
%-------avp0 ； 初始姿态信息 9*1
%-------mavp: 主惯导信息 单位：N*10 弧度 m/s 弧度 m
%output
%-------res.avp: N*10 导航信息，标准单位弧度、m/s 、m
%-------res.xkpk: 2*N 估计值与协方差阵
%% data length and time
N = length(imu(:,end));
%% kalmman参数初始化
kf = []; m = 6; n =18;
kf.m = m; kf.n = n;
kf.Pk = 1*diag([davp(1:3); davp(4:6);imuerr.eb; imuerr.db;[5;5;5]*pi/180;[2;2;2]])^2;
kf.Qt = diag([imuerr.web; imuerr.wdb;zeros(kf.n-6,1)]).^2;
kf.Gammak = eye(kf.n);
kf.I = eye(kf.n);
%% Memory allocation
kf.Kk = zeros(kf.n,kf.m);
kf.Xk = zeros(kf.n,1);
kf.Phikk_1 = zeros(kf.n,kf.n);
avp = zeros(N,10);
xkpk = zeros(N,2*kf.n);
%% Initial data
att = avp0(1:3);vel = avp0(4:6);pos = avp0(7:9);
qua = a2qua(att);
Cnb = a2mat(att);
t(1) = imu(1,end);
avp(1,:) = [att',vel',pos',t(1)];
xkpk(1,:) = [kf.Xk',diag(kf.Pk)'];
factor = 1.0;
timebar(1, N, '姿态速度匹配算法（可以估计安装误差角）.'); % 第一个参数表示间隔数，第二个参数表示总进度
%% Function realize
for i = 2:N
wbib = imu(i,1:3)' ;
fb = imu(i,4:6)' ;
dt = imu(i,end) - imu(i-1,end);
t(i) = imu(i,end);
% 传递主惯导的速度和位置
posm = mavp(i,7:9)';
vnm = mavp(i,4:6)';
% [att,vel,pos,qua,Cnb] = avp_update(wbib,fb,Cnb,qua,posm,vnm,dt);
eth = EarthParameter(posm,vnm);
vel = vel + (Cnb*fb + eth.gcc)*dt;
qua = qupdt(qua,(wbib - Cnb'*eth.wnin)*dt);
pos = posm; %装订主惯导的位置
% Kalman滤波
% 一步预测
Maa=zeros(3,3);
Maa(2)=-eth.wnin(3);
Maa(3)= eth.wnin(2);
Maa(4)= eth.wnin(3);
Maa(6)=-eth.wnin(1);
Maa(7)=-eth.wnin(2);
Maa(8)= eth.wnin(1);
Mva=zeros(3,3);
fn = Cnb*fb;
Mva(2)= fn(3);
Mva(3)=-fn(2);
Mva(4)=-fn(3);
Mva(6)= fn(1);
Mva(7)= fn(2);
Mva(8)=-fn(1);
Ft = [Maa zeros(3,3) -Cnb zeros(3,3) zeros(3,3) zeros(3,3); %
Mva zeros(3,3) zeros(3,3) Cnb zeros(3,3) zeros(3,3);
zeros(12,18);];
Fk = Ft*dt;
kf.Phikk_1 = kf.I + Fk;
kf.Xk = kf.Phikk_1*kf.Xk;
kf.Gammak(1:3,1:3) = -Cnb; kf.Gammak(4:6,4:6) = Cnb;
kf.Qk = kf.Qt*dt;
kf.Pk = kf.Phikk_1*kf.Pk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
% ------------------量测更新-----------------
% 计算姿态误差
mqnb = a2qua(mavp(i,1:3));
sqbn = qconj(qua);
Cnbm = q2mat(mqnb);
Cbsn = q2mat(sqbn);
z = Cnbm*Cbsn;
Zk = [0.5*(z(3,2)-z(2,3)); 0.5*(z(1,3)-z(3,1)); 0.5*(z(2,1)-z(1,2));
vel-vnm];
kf.Xkk_1 = kf.Xk;
kf.Pkk_1 = kf.Pk;
web = wbib- Cnb'*eth.wnie;
kf.Hk = [eye(3) zeros(3,3) zeros(3,6) -Cnb zeros(3,3);
zeros(3,3) eye(3) zeros(3,6) zeros(3,3) Cnb*askew(web)];
kf.Rk = diag([[1; 1; 1;].*pi/180/60; [0.1; 0.1; 0.1;]])^2;
kf.rk = Zk - kf.Hk*kf.Xkk_1;%残差
kf.PXZkk_1 = kf.Pkk_1*kf.Hk';%状态一步预测与量测一步预测的协均方误差
kf.PZZkk_1 = kf.Hk*kf.PXZkk_1 + kf.Rk;%量测一步预测均方误差阵
kf.Kk = kf.PXZkk_1*invbc(kf.PZZkk_1);
kf.Xk = kf.Xkk_1 + kf.Kk*kf.rk;
kf.Pk = kf.Pkk_1 - kf.Kk*kf.PZZkk_1*kf.Kk';
kf.Pk = (kf.Pk+kf.Pk')/2;
% 反馈
xk = kf.Xk;
% 姿态
qua = qdelphi(qua, factor* kf.Xk(1:3));
kf.Xk(1:3) = (1-factor)*kf.Xk(1:3);
att = q2att(qua);
Cnb = q2mat(qua);
% 速度
vel = vel-factor*kf.Xk(4:6);
kf.Xk(4:6) = (1-factor)*kf.Xk(4:6);
% 记录参数
avp(i,:) = [att',vel',pos',t(i)];
xkpk(i,:) = [xk',diag(kf.Pk)'];
timebar; % 进度条
end
%% Return Results and Save Workspace
res = varpack(avp,xkpk);
end
%% End
5、工具包中的 Demo
| 示例函数 | 描述 |
|---|---|
| Acc_Calibration_Sim_main | 加速度计标定仿真，重复性测试 |
| Acc_Calibration_main | 加速度计标定实测 |
| Gyro_Calibration_main | 陀螺仪标定仿真，重复性测试 |
| Gyro_Calibration_sim_main | 陀螺仪标定实测 |
| Integrate_navi_real_main | 抗差组合导航方法实测 |
| Integrate_navi_sim_main | 抗差组合导航方法仿真 |
| Other_main | NHC、UKF、平滑等函数测试 |
| Outrage_main | GNSS失锁方案实际数据测试 |
| Transfer_Alignment_main | 传递对准测试 |
| TrjSim_main | 仿真数据生成器 |
| data_process_main | 数据处理演示 |
| mainLeverArmEst | 杆臂估计方案测试 |
| AllanCov（文件夹） | Allan方差测试 |
** 欢迎大家学习交流，联系方式 qq: 2414342361**
