|
前言
本文给出PSINS工具箱关于滤波 适用例程：
test_SINS_GPS_153test_SINS_GPS_186test_SINS_GPS_CKF_153test_SINS_GPS_UKF_153test_SINS_GPS_PF_153等等
部分程序
【注】这一部分的重点在于给大家看看代码注释，而不是运行程序，程序基于test_SINS_GPS_153，运行结果也是一样的如果你想要完整的m文件，私戳我我留下联系方式，我可以发给你。
% PSINS组合导航153例程的注释详解
% Evand/2024-4-5/Ver1
glvs %工具箱初始化gps参数的函数
psinstypedef(153); %前者为函数名，也是初始化，后者表示使用15维状态量、3维观测量
trj = trjfile('trj10ms.mat'); %加载轨迹和IMU原始数据
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
% nn是nnts的第一个输入量，是子采样数（目前这个概念没用到，保持1），ts是imu采样周期（nnts的第二个输入量），nts是两者相乘。
imuerr = imuerrset(0.03, 100, 0.001, 5); %imuerrset这函数在设置IMU的误差项，点开里面有英文注释
imu = imuadderr(trj.imu, imuerr); %IMU原始数据（无误差）加上imuerr（误差），得到含噪的IMU数据
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]); %定义avp的初始误差
ins = insinit(avpadderr(trj.avp0,davp0), ts);
% ts是IMU采样周期，初始化是根据前面设置的数字，定义ins这个结构体，里面存了关于INS的数据
% KF filter
rk = poserrset([1;1;3]);
% 定义位置观测的误差
kf = kfinit(ins, davp0, imuerr, rk);
% 卡尔曼滤波初始化，kf是滤波相关的数据
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2; kf.pconstrain=1;
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, '15-state SINS/GPS Simulation.'); %生产timebar
ki = 1;
for k=1:nn:len-nn+1
k1 = k+nn-1;
wvm = imu(k:k1,1:6); t = imu(k1,end);
% wvm是IMU的角速度和加速度（3轴+3轴），imu(k1,end)用于取imu的时间戳
ins = insupdate(ins, wvm);
% ins的一步更新。用于上一时刻的ins值和当前的角速度、加速度来计算下一时刻的avp，因为是纯INS进行计算的，所以是ins_pure
kf.Phikk_1 = kffk(ins);
kf = kfupdate(kf);
if mod(t,1)==0
posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1); % GPS pos simulation with some white noise
kf = kfupdate(kf, ins.pos-posGPS, 'M');
[kf, ins] = kffeedback(kf, ins, 1, 'avp');
avp(ki,:) = [ins.avp', t];
xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]'; ki = ki+1;
end
timebar;
end
avp(ki:end,:) = []; xkpk(ki:end,:) = [];
% show results
insplot(avp);
avperr = avpcmpplot(trj.avp, avp);
kfplot(xkpk, avperr, imuerr);
|