├── Tools ├── fplot.m ├── rmse.m ├── Aligni0.m ├── KF_Phi.m ├── xkplot.m ├── Myavp2imu.m ├── avp_update.m ├── genImuAsb.m ├── static_pos.m ├── Arw_Vrw2std.m ├── mxGetGravity.m ├── slideVarStd.m ├── EarthParameter.m ├── globalParameter.m ├── feed_back_correct.m ├── Expand_axis_fill_figure.m ├── mToolLatLonErrorMeters.m ├── reverseEarthParameter.m └── WaveDenoise.m ├── UKF ├── UKF156.m ├── ukf_filter.m ├── utChange.m ├── state_function.m ├── UKFParameter2.m └── UKFParameter.m ├── AllanCov ├── allan.m ├── allan方差.ppt ├── gyro_data.m └── nihe.m ├── Main ├── Other_main.m ├── TrjSim_main.m ├── Outrage_main.m ├── mainLeverArmEst.m ├── data_process_main.m ├── Acc_Calibration_main.m ├── Gyro_Calibration_main.m ├── Acc_Calibration_Sim_main.m ├── Integrate_navi_real_main.m ├── Integrate_navi_sim_main.m ├── Transfer_Alignment_main.m └── Gyro_Calibration_sim_main.m ├── SINS └── ins_inpure.m ├── Filter_Algorithm ├── kf.m ├── ARkf.m ├── MRkf.m ├── kf0.m ├── SW_KF.m ├── kf_phi.m ├── RobustKf.m ├── SH_KF156.m ├── Adaptivekf156.m ├── Adaptivekf186.m ├── Adaptivekf196.m ├── BayesianFilter.m ├── SlideWindow_Rkf.m └── Virtual_Lever_Arm.m ├── Smooth ├── smooth_RTS.m ├── smooth_TKF.m ├── smooth_RTS_all.m └── RTS.m ├── Data ├── SimData │ ├── SimuAcc.m │ └── TrjSim_INS.m ├── RealData.mat ├── f1savp.mat ├── trjNm_R11.mat ├── trjRbs.mat ├── AccBiasSimu.mat ├── imu1CaliMethodsPara.mat └── imuSTGnss0608_F1.mat ├── Gyro_Calibration ├── mgenRot.m ├── mfunRef43.m ├── kfCalibrated_Xu.m └── mGyro6PosCalibration.m ├── Outrage_Solution ├── VmP_NHC.m ├── VnP_NHC.m ├── VmP_NHC_RTS.m ├── AttAddKmtcCons.m ├── ZeroVelocity_Kf.m ├── Att_Vel_AddKmtcCons.m ├── P_AdaptiveKf_NHC_PR.m └── Att_Vel_AddKmtcCons1.m ├── Wavelet_Transform ├── level.m ├── main.m ├── snrr.m ├── Gnoisegen.m └── levelandth1.m ├── Acc_Calibration ├── GA算法框架 │ ├── code.m │ ├── erro.m │ ├── select.m │ ├── test.m │ ├── GA_cross.m │ ├── GA_main.m │ ├── GA_mutation.m │ ├── searchfun.m │ └── fitness_change.m ├── mAccCaliHxJx1.m ├── mAccCaliDescent.m └── mAcce6PosCalibration.m ├── Non-holonomic_Constraint ├── NHC.m ├── UKF_constrait.m └── Vehicle_Constraint.m ├── Transfer_Alignment ├── VPmaster.m ├── SINS153Vel_TransferAlignment.m └── SINS186Att_Vel_TransferAlignment.m ├── .gitattributes └── README.md /Tools/fplot.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/fplot.m -------------------------------------------------------------------------------- /Tools/rmse.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/rmse.m -------------------------------------------------------------------------------- /UKF/UKF156.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/UKF/UKF156.m -------------------------------------------------------------------------------- /AllanCov/allan.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/AllanCov/allan.m -------------------------------------------------------------------------------- /Tools/Aligni0.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/Aligni0.m -------------------------------------------------------------------------------- /Tools/KF_Phi.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/KF_Phi.m -------------------------------------------------------------------------------- /Tools/xkplot.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/xkplot.m -------------------------------------------------------------------------------- /UKF/ukf_filter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/UKF/ukf_filter.m -------------------------------------------------------------------------------- /UKF/utChange.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/UKF/utChange.m -------------------------------------------------------------------------------- /Main/Other_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Other_main.m -------------------------------------------------------------------------------- /Main/TrjSim_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/TrjSim_main.m -------------------------------------------------------------------------------- /SINS/ins_inpure.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/SINS/ins_inpure.m -------------------------------------------------------------------------------- /Tools/Myavp2imu.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/Myavp2imu.m -------------------------------------------------------------------------------- /Tools/avp_update.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/avp_update.m -------------------------------------------------------------------------------- /Tools/genImuAsb.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/genImuAsb.m -------------------------------------------------------------------------------- /Tools/static_pos.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/static_pos.m -------------------------------------------------------------------------------- /AllanCov/allan方差.ppt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/AllanCov/allan方差.ppt -------------------------------------------------------------------------------- /AllanCov/gyro_data.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/AllanCov/gyro_data.m -------------------------------------------------------------------------------- /Filter_Algorithm/kf.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/kf.m -------------------------------------------------------------------------------- /Main/Outrage_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Outrage_main.m -------------------------------------------------------------------------------- /Smooth/smooth_RTS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Smooth/smooth_RTS.m -------------------------------------------------------------------------------- /Smooth/smooth_TKF.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Smooth/smooth_TKF.m -------------------------------------------------------------------------------- /Tools/Arw_Vrw2std.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/Arw_Vrw2std.m -------------------------------------------------------------------------------- /Tools/mxGetGravity.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/mxGetGravity.m -------------------------------------------------------------------------------- /Tools/slideVarStd.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/slideVarStd.m -------------------------------------------------------------------------------- /UKF/state_function.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/UKF/state_function.m -------------------------------------------------------------------------------- /Data/SimData/SimuAcc.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Data/SimData/SimuAcc.m -------------------------------------------------------------------------------- /Filter_Algorithm/ARkf.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/ARkf.m -------------------------------------------------------------------------------- /Filter_Algorithm/MRkf.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/MRkf.m -------------------------------------------------------------------------------- /Filter_Algorithm/kf0.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/kf0.m -------------------------------------------------------------------------------- /Main/mainLeverArmEst.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/mainLeverArmEst.m -------------------------------------------------------------------------------- /Smooth/smooth_RTS_all.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Smooth/smooth_RTS_all.m -------------------------------------------------------------------------------- /Tools/EarthParameter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/EarthParameter.m -------------------------------------------------------------------------------- /Tools/globalParameter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/globalParameter.m -------------------------------------------------------------------------------- /Data/SimData/TrjSim_INS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Data/SimData/TrjSim_INS.m -------------------------------------------------------------------------------- /Filter_Algorithm/SW_KF.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/SW_KF.m -------------------------------------------------------------------------------- /Filter_Algorithm/kf_phi.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/kf_phi.m -------------------------------------------------------------------------------- /Gyro_Calibration/mgenRot.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Gyro_Calibration/mgenRot.m -------------------------------------------------------------------------------- /Main/data_process_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/data_process_main.m -------------------------------------------------------------------------------- /Outrage_Solution/VmP_NHC.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Outrage_Solution/VmP_NHC.m -------------------------------------------------------------------------------- /Outrage_Solution/VnP_NHC.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Outrage_Solution/VnP_NHC.m -------------------------------------------------------------------------------- /Tools/feed_back_correct.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/feed_back_correct.m -------------------------------------------------------------------------------- /Wavelet_Transform/level.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Wavelet_Transform/level.m -------------------------------------------------------------------------------- /Wavelet_Transform/main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Wavelet_Transform/main.m -------------------------------------------------------------------------------- /Wavelet_Transform/snrr.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Wavelet_Transform/snrr.m -------------------------------------------------------------------------------- /Filter_Algorithm/RobustKf.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/RobustKf.m -------------------------------------------------------------------------------- /Filter_Algorithm/SH_KF156.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/SH_KF156.m -------------------------------------------------------------------------------- /Gyro_Calibration/mfunRef43.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Gyro_Calibration/mfunRef43.m -------------------------------------------------------------------------------- /Main/Acc_Calibration_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Acc_Calibration_main.m -------------------------------------------------------------------------------- /Main/Gyro_Calibration_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Gyro_Calibration_main.m -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/code.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/code.m -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/erro.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/erro.m -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/select.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/select.m -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/test.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/test.m -------------------------------------------------------------------------------- /Acc_Calibration/mAccCaliHxJx1.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/mAccCaliHxJx1.m -------------------------------------------------------------------------------- /Main/Acc_Calibration_Sim_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Acc_Calibration_Sim_main.m -------------------------------------------------------------------------------- /Main/Integrate_navi_real_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Integrate_navi_real_main.m -------------------------------------------------------------------------------- /Main/Integrate_navi_sim_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Integrate_navi_sim_main.m -------------------------------------------------------------------------------- /Main/Transfer_Alignment_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Transfer_Alignment_main.m -------------------------------------------------------------------------------- /Non-holonomic_Constraint/NHC.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Non-holonomic_Constraint/NHC.m -------------------------------------------------------------------------------- /Outrage_Solution/VmP_NHC_RTS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Outrage_Solution/VmP_NHC_RTS.m -------------------------------------------------------------------------------- /Tools/Expand_axis_fill_figure.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/Expand_axis_fill_figure.m -------------------------------------------------------------------------------- /Tools/mToolLatLonErrorMeters.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/mToolLatLonErrorMeters.m -------------------------------------------------------------------------------- /Tools/reverseEarthParameter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Tools/reverseEarthParameter.m -------------------------------------------------------------------------------- /Transfer_Alignment/VPmaster.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Transfer_Alignment/VPmaster.m -------------------------------------------------------------------------------- /Wavelet_Transform/Gnoisegen.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Wavelet_Transform/Gnoisegen.m -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | *.mat filter=lfs diff=lfs merge=lfs -text 2 | allan_analysis.txt filter=lfs diff=lfs merge=lfs -text 3 | -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/GA_cross.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/GA_cross.m -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/GA_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/GA_main.m -------------------------------------------------------------------------------- /Acc_Calibration/mAccCaliDescent.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/mAccCaliDescent.m -------------------------------------------------------------------------------- /Filter_Algorithm/Adaptivekf156.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/Adaptivekf156.m -------------------------------------------------------------------------------- /Filter_Algorithm/Adaptivekf186.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/Adaptivekf186.m -------------------------------------------------------------------------------- /Filter_Algorithm/Adaptivekf196.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/Adaptivekf196.m -------------------------------------------------------------------------------- /Filter_Algorithm/BayesianFilter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/BayesianFilter.m -------------------------------------------------------------------------------- /Main/Gyro_Calibration_sim_main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Main/Gyro_Calibration_sim_main.m -------------------------------------------------------------------------------- /Outrage_Solution/AttAddKmtcCons.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Outrage_Solution/AttAddKmtcCons.m -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/GA_mutation.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/GA_mutation.m -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/searchfun.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/searchfun.m -------------------------------------------------------------------------------- /Filter_Algorithm/SlideWindow_Rkf.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/SlideWindow_Rkf.m -------------------------------------------------------------------------------- /Filter_Algorithm/Virtual_Lever_Arm.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Filter_Algorithm/Virtual_Lever_Arm.m -------------------------------------------------------------------------------- /Gyro_Calibration/kfCalibrated_Xu.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Gyro_Calibration/kfCalibrated_Xu.m -------------------------------------------------------------------------------- /Outrage_Solution/ZeroVelocity_Kf.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Outrage_Solution/ZeroVelocity_Kf.m -------------------------------------------------------------------------------- /Acc_Calibration/mAcce6PosCalibration.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/mAcce6PosCalibration.m -------------------------------------------------------------------------------- /Outrage_Solution/Att_Vel_AddKmtcCons.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Outrage_Solution/Att_Vel_AddKmtcCons.m -------------------------------------------------------------------------------- /Outrage_Solution/P_AdaptiveKf_NHC_PR.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Outrage_Solution/P_AdaptiveKf_NHC_PR.m -------------------------------------------------------------------------------- /Acc_Calibration/GA算法框架/fitness_change.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Acc_Calibration/GA算法框架/fitness_change.m -------------------------------------------------------------------------------- /Gyro_Calibration/mGyro6PosCalibration.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Gyro_Calibration/mGyro6PosCalibration.m -------------------------------------------------------------------------------- /Non-holonomic_Constraint/UKF_constrait.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Non-holonomic_Constraint/UKF_constrait.m -------------------------------------------------------------------------------- /Outrage_Solution/Att_Vel_AddKmtcCons1.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Outrage_Solution/Att_Vel_AddKmtcCons1.m -------------------------------------------------------------------------------- /Non-holonomic_Constraint/Vehicle_Constraint.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Non-holonomic_Constraint/Vehicle_Constraint.m -------------------------------------------------------------------------------- /Transfer_Alignment/SINS153Vel_TransferAlignment.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Transfer_Alignment/SINS153Vel_TransferAlignment.m -------------------------------------------------------------------------------- /Data/RealData.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:650bf74d18bd39a7811a7d0c41599b9c00486d4ed008a76783f64b7ef9523e9c 3 | size 9977524 4 | -------------------------------------------------------------------------------- /Data/f1savp.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:a2a37d40752f18f229f20550e61562fdae51769e7e1f977b3c509844953c2f73 3 | size 33633396 4 | -------------------------------------------------------------------------------- /Data/trjNm_R11.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:f15ffa8d80a64ca4aa2a1d199b9835173558b18a063ee469e4e575b292774b0c 3 | size 23969102 4 | -------------------------------------------------------------------------------- /Data/trjRbs.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:dfd02db96853a3bbc8ec26695c0abfa930322343d6df5b7446393342d6514356 3 | size 3984449 4 | -------------------------------------------------------------------------------- /Transfer_Alignment/SINS186Att_Vel_TransferAlignment.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zelanzou/NaveCodePro/HEAD/Transfer_Alignment/SINS186Att_Vel_TransferAlignment.m -------------------------------------------------------------------------------- /Data/AccBiasSimu.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:1b0eb8d7663ab6b0a2eb9c89093ceaf2797dbe97d9c3cb2cf15254737593a1ab 3 | size 2232536 4 | -------------------------------------------------------------------------------- /Data/imu1CaliMethodsPara.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:604e9622a165a99aa7b2dc64809b9b50f3c513b7d79cc892e65110889b845212 3 | size 203411 4 | -------------------------------------------------------------------------------- /Data/imuSTGnss0608_F1.mat: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:860c5790c0915684b6c7673f6704b07df99417b100f5b8bc7fb46eebee09c198 3 | size 57891501 4 | -------------------------------------------------------------------------------- /Wavelet_Transform/levelandth1.m: -------------------------------------------------------------------------------- 1 | function snrwave=levelandth1(y,s,wave,n) 2 | for j=1:n 3 | xdh=wden(s,'sqtwolog','s','mln',5,wave(j,:)); 4 | snrwave(j)=snrr(y,xdh); 5 | end 6 | end 7 | -------------------------------------------------------------------------------- /AllanCov/nihe.m: -------------------------------------------------------------------------------- 1 | function C=nihe(tau,sig,M) 2 | X=tau'; 3 | Y=sig'; 4 | B=zeros(1,2*M+1); 5 | F=zeros(length(X),2*M+1); 6 | 7 | for i=1:2*M+1 8 | kk=i-M-1; 9 | F(:,i)=X.^kk; 10 | end 11 | 12 | A=F'*F; 13 | B=F'*Y; 14 | C=A\B; 15 | -------------------------------------------------------------------------------- /UKF/UKFParameter2.m: -------------------------------------------------------------------------------- 1 | function [gamma,Wm,Wc] = UKFParameter2(n) 2 | beta = 2; kapa = 0;alpha = 0.001; 3 | lamda = alpha^2*(n+kapa)-n; 4 | gamma = sqrt( n+ lamda); 5 | m = gamma^2; 6 | Wm = [lamda/m; repmat(1 /(2*m),2*n,1)]; 7 | Wc = [lamda/m + 1 +beta - alpha^2;Wm(2:end)]; 8 | end -------------------------------------------------------------------------------- /Smooth/RTS.m: -------------------------------------------------------------------------------- 1 | function [Xs,Ps] = RTS(F,xf,Pf,xf1,Pf1) 2 | Xs = xf1;Ps = Pf1; 3 | for k = length(F)-1:-1:1 4 | Ks = Pf1{k}*F{k}'*invbc(Pf{k+1}); 5 | Xs{k} = xf1{k} + Ks*(Xs{k+1}-xf{k+1}); 6 | Ps{k} = Pf1{k} + Ks*(Ps{k+1} - Pf{k+1})*Ks'; 7 | end 8 | end -------------------------------------------------------------------------------- /UKF/UKFParameter.m: -------------------------------------------------------------------------------- 1 | function [gamma,Wm,Wc] = UKFParameter(n) 2 | beta = 2; kapa = 0;alpha = 0.001; 3 | lamda = alpha^2*(n+kapa)-n; 4 | gamma = sqrt( n+ lamda); 5 | m = gamma^2; 6 | Wm = [lamda/m; repmat(1 /(2*m),2*n,1)]; 7 | Wc = eye(2*n+1)*1 /(2*m); 8 | Wc(1,1) = lamda/m + 1 +beta - alpha^2; 9 | end -------------------------------------------------------------------------------- /Tools/WaveDenoise.m: -------------------------------------------------------------------------------- 1 | function imu = WaveDenoise(imu) 2 | s1 = imu(:,1);s2 = imu(:,2);s3 = imu(:,3); 3 | s4 = imu(:,4);s5 = imu(:,5);s6 = imu(:,6); 4 | % thrrr3='heursure'; 5 | wavec='sym4'; 6 | M3 = 5 ;% 7 | xds1=wden(s1,'heursure','s','mln',M3,wavec); 8 | xds2=wden(s2,'heursure','s','mln',M3,wavec); 9 | xds3=wden(s3,'heursure','s','mln',M3,wavec); 10 | xds4=wden(s4,'heursure','s','mln',M3,wavec); 11 | xds5=wden(s5,'heursure','s','mln',M3,wavec); 12 | xds6=wden(s6,'heursure','s','mln',M3,wavec); 13 | imu = [xds1 xds2 xds3 xds4 xds5 xds6 imu(:,7)]; 14 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # NaveCodePro 2 | INS/GNSS Integration navigation; Contain Inpure navigation， Integration navigation，Transfer Alignment，Robuster Filter,NHC,etc. 3 | 4 | # NaveCodePro使用说明 5 | 6 | 1，本代码工具箱需要在严恭敏老师的PSINS工具箱的基础上进行使用 7 | 8 | ​ PSINS相关资料参考严老师网站：http://www.psins.org.cn/ 9 | 10 | 2，NaveCodePro在使用时需要所有文件全部选中在当前路径，以防缺少函数 11 | 12 | 3，NaveCode后期的维护和更新均上传至GitHub：https://github.com/zelanzou/NaveCodePro 13 | 14 | ​ 部分数据由于过大，上传至百度网盘：链接：https://pan.baidu.com/s/1VdDXrftKy3uswVjWyirzbQ ，提取码：oajl 15 | 16 | # NaveCodePro函数说明 17 | 18 | | 函数名称 | 函数描述 | 参考文献 | 19 | | -------------------------------- | ------------------------------------------------------------ | -------- | 20 | | mAccCaliDescent | 基于加速度计矢量模相等的标定方法，代价函数值下降的搜索方法 | [1] | 21 | | mAccCaliHxJx1 | 加速度计模型计算雅各比和海森矩阵 | | 22 | | mAcce6PosCalibration | 六位置修正加速度计标定法 | [2] | 23 | | GA算法框架（文件夹内所有函数） | 遗传算法，可以用于非线性最优规划 | | 24 | | ARkf | 马氏距离单因子抗差算法 | [4],[9] | 25 | | Adaptivekf156 | 序贯量测+方差受限算法 15*6维，估计零偏 | | 26 | | Adaptivekf186 | 序贯量测+方差受限算法 18*6维 ，估计零偏，杆臂 | | 27 | | Adaptivekf196 | 序贯量测+方差受限算法 18*6维 ，估计零偏，杆臂，同步时间 | | 28 | | BayesianFilter | 受污染的贝叶斯滤波算法 | [5] | 29 | | MRkf | 马氏距离单因子修正抗差算法 | [3] | 30 | | RobustKf | 多因子自适应抗差算法 | | 31 | | SH_KF156 | Sage-Husa自适应卡尔曼滤波（序贯量测） | | 32 | | SlideWindow_Rkf | 基于滑动窗，马氏距离检测滤波算法 | | 33 | | kf | 标准卡尔曼滤波 | | 34 | | kf0 | 标准卡拉曼滤波，更准确的时间融合方式 | | 35 | | kf_phi | 增加航向观测的kalman | | 36 | | Virtual_Lever_Arm | 虚拟杆臂法估计杆臂 | [6] | 37 | | kfCalibrated_Xu | 两步修正法标定陀螺 | [7] | 38 | | mGyro6PosCalibration | 六位置法标定陀螺 | | 39 | | mfunRef43 | 系统级标定法 | [8] | 40 | | mgenRot | 生成旋转矩阵 | | 41 | | NHC | 车辆不完整约束 | | 42 | | UKF_constrait | ukf用于车辆不完整约束 | | 43 | | Vehicle_Constraint | 高程+航向+不完整约束 | | 44 | | AttAddKmtcCons | gnss outrage 解决方案: 姿态匹配+ 车体运动学速度匹配 20维传递对准算法 估计主子安装角，车体安装角，不能估计杆臂 | | 45 | | Att_Vel_AddKmtcCons | gnss outrage 解决方案:姿态匹配+速度匹配 23维传递对准算法 估计主子安装角，杆臂，车体安装角 | | 46 | | Att_Vel_AddKmtcCons1 | gnss outrage 解决方案:增加正常段使用车辆约束 | | 47 | | P_AdaptiveKf_NHC_PR | 17维位置组合卡尔曼滤波 ，考虑GNSS缺失情况下使用运动学约束 , 模式识别（PR） | | 48 | | VmP_NHC | 车体速度辅助+位置，失锁时使用NHC | | 49 | | VmP_NHC_RTS | 车体速度辅助+位置，失锁时使用NHC/RTS | [11] | 50 | | VnP_NHC | 导航速度辅助+位置，失锁时使用NHC | | 51 | | ZeroVelocity_Kf | 零速修正kalman滤波 | | 52 | | ins_inpure | 纯惯性导航 | | 53 | | RTS | RTS平滑核心代码 | | 54 | | smooth_RTS | RTS平滑，平滑区间与外部观测频率一直 | | 55 | | smooth_RTS_all | RTS平滑，平滑区间取全段数据 | | 56 | | smooth_TKF | 双向平滑 | [9] | 57 | | Aligni0 | 粗对准 | | 58 | | Arw_Vrw2std | ARW, VRW 与 噪声标准差的转换 | | 59 | | EarthParameter | 地球参数 | | 60 | | Expand_axis_fill_figure | 去除画图的空白边界 | | 61 | | KF_Phi | 组合导航离散线性模型 | | 62 | | Myavp2imu | 通过avp反向生成imu数据 | | 63 | | WaveDenoise | imu小波去噪 | | 64 | | avp_update | 导航更新 | | 65 | | feed_back_correct | 反馈校正 | | 66 | | fplot | 计算导航误差并画图 | | 67 | | genImuAsb | 产生非正交变换矩阵，从理想 b 系到 传感器系 | | 68 | | globalParameter | 地球参数全局变量 | | 69 | | mToolLatLonErrorMeters | 弧度转化为米 | | 70 | | mxGetGravity | 计算重力值 | | 71 | | reverseEarthParameter | 反向滤波更新地球参数值 | | 72 | | rmse | 计算rmse | | 73 | | slideVarStd | 滑动计算数据的方差、标准差，均值 | | 74 | | xkplot | 滤波估计值和协方差画图 | | 75 | | SINS153Vel_TransferAlignment | 速度匹配15维传递对准算法 ，仅估计杆臂 | | 76 | | SINS186Att_Vel_TransferAlignment | 姿态速度匹配18维传递对准算法 ，估计安装角和杆臂 | | 77 | | VPmaster | 主惯导速度位置匹配 | | 78 | | UKF156 | 组合导航15维ukf | | 79 | | UKFParameter | ukk参数设置 | | 80 | | state_function | ukf的非线性模型 | | 81 | | ukf_filter | ukf滤波 | | 82 | | utChange | ut变化 | | 83 | | Wavelet_Transform（文件夹） | 小波变换 | | 84 | | SimuAcc | 加速度计仿真数据发生器 | | 85 | | TrjSim_INS | 轨迹发生器 | | 86 | 87 | # NaveCodePro Demos 88 | 89 | | 函数名称 | 函数描述 | 参考文献 | 90 | | ------------------------- | ---------------------------- | -------- | 91 | | Acc_Calibration_Sim_main | 加速度计标定仿真，重复性测试 | | 92 | | Acc_Calibration_main | 加速度计标定实测 | | 93 | | Gyro_Calibration_main | 陀螺仪标定仿真，重复性测试 | | 94 | | Gyro_Calibration_sim_main | 陀螺仪标定实测 | | 95 | | Integrate_navi_real_main | 抗差组合导航方法实测 | | 96 | | Integrate_navi_sim_main | 抗差组合导航方法仿真 | | 97 | | Other_main | NHC,UKF,平滑等函数测试 | | 98 | | Outrage_main | GNSS失锁方案实际数据测试 | | 99 | | Transfer_Alignment_main | 传递对准测试 | | 100 | | TrjSim_main | 产生仿真数据 | | 101 | | data_process_main | 数据处理 | | 102 | | mainLeverArmEst | 杆臂估计方案测试 | | 103 | | AllanCov（文件夹） | Allan方差测试 | | 104 | 105 | # 参考文献 106 | 107 | [1] Frosio I, Pedersini F, Borghese N A. Autocalibration of Triaxial MEMS Accelerometers With Automatic Sensor Model Selection[J]. IEEE Sensors Journal,2012,12(6):2100-2108. 108 | 109 | [2] Xu T, Xu X, Xu D, et al. A Novel Calibration Method Using Six Positions for MEMS Triaxial Accelerometer[J]. IEEE Transactions on Instrumentation and Measurement, 2020, 70: 1-11. 110 | 111 | [3] Chen J , Shu-Bi Z . A Novel Adaptively-Robust Strategy Based on the Mahalanobis Distance for GPS/INS Integrated Navigation Systems[J]. Sensors, 2018, 18(3):695. 112 | 113 | [4]. Chang G . Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion[J]. Journal of Geodesy, 2014, 88(4):391-401. 114 | 115 | [5]. 孙增析，邓志东, 一种对成片连续野值不敏感的鲁棒滤波. 清华大学学报（自然科学版）, 1994. 116 | 117 | [6] Borko, A., I. Klein and G. Even-Tzur, GNSS/INS Fusion with Virtual Lever-Arm Measurements. Sensors, 2018. 18(7): p. 2228. 118 | 119 | [7]邹泽兰，徐同旭，徐祥，赵鹤鸣. 基于两步修正法的MEMS三轴陀螺仪标定方法[j],仪器仪表学报 120 | 121 | [8]Zhou Q, Yu G, Li H, et al. A Novel MEMS Gyroscope In-Self Calibration Approach[J]. Sensors, 2020, 20(18): 5430. 122 | 123 | [9]Liu H, Nassar S, El-Sheimy N. Two-filter smoothing for accurate ins/gps land-vehicle navigation in urban centers [J]. IEEE Transactions on Vehicular Technology, 2010, 59(9): 4256-4267 124 | 125 | [10] Yang Y X, Cui X Q. Adaptively robust filter with multi adaptive factors [J]. Survey Review, 2008, 40(309): 260-270. 126 | 127 | [11] Hang G , Guo J , Min Y , et al. A weighted combination filter with nonholonomic constrains for integrated navigation systems[J]. Advances in Space Research, 2015, 55(5):1470-1476. 128 | 129 | # 致谢 130 | 131 | NaveCodePro是在严恭敏老师的工具箱衍生而来，代码均与组合导航相关，code思路为文献复现和本人的原创设计。感谢课题组徐祥老师，徐同旭师兄对NavecodePro中部分代码的贡献。 132 | 133 | # 版权说明 134 | 135 | NaveCodePro不涉及也不允许任何商用，代码为本人对三年研究课题的整理，如有代码疑问或侵权行为，可联系邮箱：zelanzou@163.com. 136 | --------------------------------------------------------------------------------