glvs; nn = 2;ts = 0.1; nts = nn*ts;% 子样数和采样时间 att0 = [0; 0; 30]*arcdeg; qnb0 = a2qua(att0); vn0 = [0;0;0]; pos0 = [34*arcdeg; 108*arcdeg; 100]; qnb = qnb0; vn = vn0; pos = pos0;% 姿态、速度和位置初始化 eth = earth(pos, vn); wm = qmulv(qconj(qnb),eth.wnie)*ts: vm = qmulv(qconj(qnb),-eth.gn)*ts wm = repmat(wm', nn, 1); vm = repmat(vm', nn, 1); % 仿真静态IMU数据 phi = [0.1; 0.2; 3]*arcmin: qnb = qaddphi(qnb, phi) % 失准角 eb =[0.01;0.015;0.02]*dph; web = [0.001;0.001;0.001]*dpsh; % 陀螺常值零偏,角度随机游走 系数 db = [80;90;100]*ug; wdb = [1;1;1]*ugpsHz; % 加速度计常值偏值,速度随机游走系数 Qk = diag([web; wdb; zeros(9,1)])/2*nts; rk = [[0.1;0.1;0.1];[[10;10]/Re;10]] Rk = diag(rk)/2; "zv([6n*[001:001:001] 'udpx[L'0:L'0:L'0] :[ol:ad/[0l:0l]] :[L:L:L] :Bapoyex[0L:L'0:L'0]])be!p = 0d Hk = [zeros(6,3),eye(6),zeros(6)] kf = kfinit(Qk, Rk, P0, zeros(15), Hk); % kf滤波器初始化 len = fix(3600/ts) % 仿真时长 kf = kfupdate(kf) if mod(t,1)<nts gps = [vn0; pos0] + rk.*randn(6,1); % GPS速度位置仿真 kf = kfupdate(kf, [vn;pos]-gps, 'M'); vn(3) = vn(3)- kf.Xk(6); Kt.XK(6) = O % 反馈 end avp(kk,:) = [qq2phi(qnb,qnb0); vn; pos; t]'; xkpk(kk,:) = [kf.Xk; diag(kf.Pk); t]; kk = kk+1; if mod(t,100)<nts disp(fix(t)); end % 显示进度 end avp(kk:end,:) = []; xkpk(kk:end,:) = []: tt = avp(:,end); % 状态真值与估计效果对比佟 mysubplot(321, tt, [avp(:,1:2),xkpk(:,1:2)]/arcmin, '\phi_E,\phi_N /\prime'); mysubplot(322, tt, [avp(:,3),xkpk(:,3)]/arcmin, '\phi_U /\prime'); mysubplot(323, tt, [avp(:,4:6),xkpk(:,4:6)], '\deltav /n /m/s'); mysubplot(324,t,ideltapos(avp(:7:9)),[xkpk(:,7),xkpk(:,8).*cos(avp(:,7))]*Re,xkpk(:,9)],\DeltaP m'); mysubplot(325, tt, xkpk(:,10:12)/dph, '\epsilon /\circ/h'); mysubplot(326, t, xkpk(:,13:15)/ug, '\nabla / ug'); % 均方差收敛佟 pk = sqrt(xkpk(:,16:end-1)) mysubplot(321, tt, pk(:,1:2)/arcmin, '\phi_E,\phi_N /\prime'); mysubplot(322, tt, pk(:,3)/arcmin, '\phi_U /\prime'): mysubplot(323, tt, pk(:,4:6), '\deltav in / m/s'); mysubplot(324, t, [[pk(:,7),pk(:,8)*cos(avp(1,7))]*Re,pk(:,9)], \DeltaP /m'); mysubplot(325, tt, pk(:,10:12)/dph, '\epsilon /\circ/h'); mysubplot(326, tt, pk(:,13:15)/ua, "\nabla / ua'):
时间: 2024-04-17 13:27:22 浏览: 179
psins简化版150914.rar_SINS_glvs工具箱_惯导工具箱_捷联惯导算法工具箱_简化惯导
5星 · 资源好评率100%
这段代码是一个姿态和导航的仿真,主要包括以下几个步骤:
1. 定义了一些变量和常数,如子样数和采样时间。
2. 初始化姿态、速度和位置。
3. 生成静态IMU数据。
4. 设置失准角。
5. 定义陀螺常值零偏和角度随机游走系数,以及加速度计常值偏值和速度随机游走系数。
6. 计算状态方程和观测方程的噪声协方差矩阵。
7. 初始化卡尔曼滤波器。
8. 定义仿真时长。
9. 在循环中更新卡尔曼滤波器,并进行GPS速度和位置的仿真。
10. 保存状态真值与估计效果对比的结果。
11. 保存均方差收敛的结果。
最后,代码通过绘图显示了姿态、速度、位置等参数的变化情况,并展示了均方差的收敛情况。
阅读全文