强跟踪UKF滤波实现捷联惯导实现初始对准.rar

  • xintingduiqi
    了解作者
  • matlab
    开发工具
  • 7KB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 1 积分
    下载积分
  • 21
    下载次数
  • 2018-10-19 16:16
    上传日期
强跟踪UKF滤波实现捷联惯导实现初始对准
强跟踪UKF滤波实现捷联惯导实现初始对准.rar
  • stukf惯导初始对准
  • getf.m
    1.3KB
  • qconj.m
    52B
  • a2cnb.m
    297B
  • a2caw.m
    211B
  • m2qnb.m
    382B
  • rv2q.m
    581B
  • glvs.m
    1.1KB
  • askew.m
    131B
  • qq2phi.m
    87B
  • q2att.m
    55B
  • a2qnb.m
    55B
  • qmul.m
    327B
  • STFUKFfangzh.m
    10.2KB
  • m2att.m
    138B
  • qmulv.m
    102B
  • qaddphi.m
    67B
  • a2cwa.m
    193B
  • kfdis.m
    683B
  • earth.m
    430B
内容介绍
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 参照《SINS非线性自对准中的强跟踪UKF算法设计》,运用十二维模型 % 以速度为观测量,运用仿真的数据编写的程序 % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % Hybrid extended Kalman filter example. % Track a body falling through the atmosphere. % This example is taken from [Jul00], which was based on [Ath68]. close glvs pos=[ 34.2478780*glv.deg;108*glv.deg;400]; att = [-0.50354; 0.16541; -9.14793]*glv.deg; phi = [2.1; 8.8; -10]*glv.min; qnb0 = qaddphi(a2qnb(att), phi); % 姿态真值-0.36554 0.17708 -10.28405 phi = [6; 6; 300]*glv.min; qnb = qaddphi(qnb0, phi); % 加入姿态误差 Lat = 34.2478780*glv.deg; cnb=q2cnb(qnb); L = Lat ; h=400; sl=sin(Lat); cl=cos(Lat); tl=sl/cl; sl2=sl*sl; sl4=sl2*sl2; sq = 1-glv.e2*sl2; sq2 = sqrt(sq); Rm = glv.Re*(1-glv.e2)/sq/sq2; Rn = glv.Re/sq2; wnie = glv.wie*[0; cos(Lat); sin(Lat)]; wbib = qmulv(qconj(qnb0),wnie); gn = [0; 0; -glv.g0]; fb = qmulv(qconj(qnb0),-gn); Gw = dlmread('ImuData2.txt',' ',[100,1,100000,3]); %陀螺输出角增量矩阵 Fb = dlmread('ImuData2.txt',' ',[100,4,100000,6]); %加速度计输出速度增量矩阵 Sr = dlmread('ImuData2.txt',' ',[20000,7,60000,7]); %里程计输出速度 ai=[20055.41484 -0.0006202 0.00288948 -0.00022049 19690.52865 0.0009072 -0.00260317 -0.00061325 22854.80286]; A_linbu=[-2.05353527 -32.36328316 -6.79615679]; gi=[2.14408478 -0.00032211 0.00312344 0.0011928 2.14459163 0.00113527 -0.00174076 -0.00079357 2.14368441]; G_linbu=[0.09451861 -0.0111049 0.07]; %首先对各参数进行更新 eb = [0.02; 0.02; 0.02]*glv.dph; web = [0.05; 0.05; 0.05]*glv.dph; % 陀螺误差 db = [100; 100; 100]*glv.ug; wdb = [50; 50;50]*glv.ug; % 加速度计误差 % 滤波器参数,参见《捷联惯导系统静基座初始对准精度分析及仿真》 Q = diag([web;wdb; [0.1;0.1;0.1]*0*glv.dph;[100;100; 100]*0*glv.ug;])^2; R = diag([0.1;0.1;0.1])^2; x = zeros(12,1); vn= zeros(3,1); vb= zeros(3,1); P = diag([[0.1;0.1;0.5*10]*glv.deg; 0.1;0.1;0.1;[0.1;0.1;0.1]*glv.dph;[100;100; 100]*glv.ug;])^2; xhatukf = x; Pukf = P; Ts = 0.05; % measurement time step randn('state',sum(100*clock)); % random number generator seed ts=0.005; t1=1; t2=300; len1 = fix(t1/ts); len2 = fix(t2/ts); tf = 600; % simulation length (seconds) len = fix(tf/ts); dt = 0.01; % time step for integration (seconds) G=zeros(12,6); G = [ cnb(1,1) cnb(1,2) cnb(1,3) 0 0 0 0 0 0 0 0 0 cnb(2,1) cnb(2,2) cnb(2,3) 0 0 0 0 0 0 0 0 0 cnb(3,1) cnb(3,2) cnb(3,3) 0 0 0 0 0 0 0 0 0 0 0 0 cnb(1,1) cnb(1,2) cnb(1,3) 0 0 0 0 0 0 0 0 0 cnb(2,1) cnb(2,2) cnb(2,3) 0 0 0 0 0 0 0 0 0 cnb(3,1) cnb(3,2) cnb(3,3) 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]; Tuokk_1=G; xArray = x; xhatukfArray = xhatukf; Parray = diag(P); Pukfarray = diag(Pukf); wgi = [0.05, 0.05, 0.05]*10*glv.dph; % 陀螺误差 wai = [50,50,50]*10*glv.ug;%加速度记得噪声 w0=wbib*ts; v0=fb*ts; %静止时角增量和比力增量 W = ones(24,1) / 24; % UKF weights k=1; %% for l=len1:10:len2 for j=l:l+9 % Gw(j,:)= Gw(j,:)-G_linbu*ts; %零位补偿 % Fb(j,:)= Fb(j,:)-A_linbu*ts; Gw(j,:) = w0'+eb'*ts+(wgi.*randn(1,3))*ts; % 构造多子样数据 Fb(j,:) = v0'+db'*ts+(wai.*randn(1,3))*ts; end % wm = Gw(l:l+9,:)*gi^-1*glv.dph; %标定参数补偿 % wme=[-wm(:,3),wm(:,2),wm(:,1)]'; %坐标变换 % vm = Fb(l:l+9,:)*ai^-1*glv.g0 ; % vme =[-vm(:,3),vm(:,2),vm(:,1)]'; % fbe=sum(vme,2);% % wbe=sum(wme,2);% wm = Gw(l:l+9,:); %标定参数补偿*gi^-1*glv.dph wme=[wm(:,1),wm(:,2),wm(:,3)]'; %坐标变换*ai^-1*glv.g0 vm = Fb(l:l+9,:) ; vme =[vm(:,1),vm(:,2),vm(:,3)]'; fbe=sum(vme,2);% wbe=sum(wme,2);% [qnb,vn,pos] = sins(qnb,vn,pos,wbe,fbe,Ts); %导航解算 C=q2cnb(qnb); % for j=1:3 % wbe=[-wm(j,3);wm(j,2);wm(j,1)]*200; % qnb = qmul(qnb, rv2q( (wbe-qmulv(qconj(qnb),wnie))*Ts )); % 姿态更新 % vb=vb+[-vm(j,3);vm(j,2);vm(j,1)]; % vn=qmulv(qnb, vb); % end fn = 20*qmulv(qnb, fbe )+gn; wn = 20*qmulv(qnb, wbe )-wnie; z = [vn(1:3)]; % 以水平比力为观测量;fn(1:3) %% Generate the UKF sigma points. [root,p] = chol(12*Pukf); for i = 1 : 12 sigma(:,i) = xhatukf + root(i,:)'; sigma(:,i+12) = xhatukf - root(i,:)'; end for i = 1 : 24 xbreve(:,i) = sigma(:,i); end % UKF time update for i = 1 : 24 for tau = dt : dt : Ts Cw=1/cos(xbreve(1,i))*[cos(xbreve(2,i))*cos(xbreve(1,i)) 0 sin(xbreve(2,i))*cos(xbreve(1,i)) sin(xbreve(2,i))*sin(xbreve(1,i)) cos(xbreve(1,i)) -cos(xbreve(2,i))*sin(xbreve(1,i)) -sin(xbreve(2,i)) 0 cos(xbreve(2,i))]; xbrevedot(1,1) = -wnie(2)*sin(xbreve(3,i))+wnie(3)*xbreve(2,i)-xbreve(5,i)/(Rm+h)-C(1,1)*xbreve(7,i)-C(1,2)*xbreve(8,i)-C(1,3)*xbreve(9,i); xbrevedot(2,1) = (1-cos(xbreve(3,i)))*wnie(2)-wnie(3)*xbreve(1,i)+xbreve(4,i)/(Rn+h)-C(2,1)*xbreve(7,i)-C(2,2)*xbreve(8,i)-C(2,3)*xbreve(9,i); xbrevedot(3,1) = wnie(2)*(xbreve(1,i)*cos(xbreve(3,i))-xbreve(2,i)*sin(xbreve(3,i)))+xbreve(4,i)/(Rn+h)*tan(L) -C(3,1)*xbreve(7,i)-C(3,2)*xbreve(8,i)-C(3,3)*xbreve(9,i); % xbrevedot(3,1) = wnie(2)*(xbreve(1,i)*cos(xbreve(3,i)))+xbreve(4,i)/(Rn+h)*tan(L) -C(3,1)*xbreve(7,i)-C(3,2)*xbreve(8,i)-C(3,3)*xbreve(9,i); xbrevedot(1:3,1)=Cw*xbrevedot(1:3,1); xbrevedot(4,1) = -glv.g0*xbreve(2,i)-2*wnie(2)*xbreve(6,i)+2*wnie(3)*xbreve(5,i)+C(1,1)*xbreve(10,i)+C(1,2)*xbreve(11,i)+C(1,3)*xbreve(12,i); xbrevedot(5,1) = glv.g0*xbreve(1,i)-2*wnie(3)*xbreve(4,i)+C(2,1)*xbreve(10,i)+C(2,2)*xbreve(11,i)+C(2,3)*xbreve(12,i); xbrevedot(6,1) = 2*wnie(2)*xbreve(4,i)+C(3,1)*xbreve(10,i)+C(3,2)*xbreve(11,i)+C(3,3)*xbreve(12,i); xbrevedot(7,1) = 0; xbrevedot(8,1) = 0; xbrevedot(9,1) = 0; xbrevedot(10,1) = 0; xbrevedot(11,1) = 0; xbrevedot(12,1) = 0; xbreve(:,i) = xbreve(:,i) + xbrevedot * dt; end end xhatukf = zeros(12,1); for i = 1 : 24 xhatukf = xhatukf + W(i) * xbreve(:,i); end Pukf = zeros(12,12); for i = 1 : 24 Pukf = Pukf + W(i) * (xbreve(:,i) - xhatukf) * (xbreve(:,i) - xhatukf)'; end Pukf = Pukf + G*Q*G'; %第二次UT变换 [root,p] = chol(12*Pukf); for i = 1 : 12 sigma(:,i) = xhatukf + root(i,:)'; sigma(:,i+12) = xhatukf - root(i,:)'; end for i = 1 : 24 xbreve(:,i) = sigma(:,i); end % UKF measurement update for i = 1 : 24 zukf(1,i) = xbreve(4,i); zukf(2,i) = xbreve(5,i); zukf(3,i) = xbreve(6,i); end zhat = 0; for i = 1 : 24 zhat = zhat + W(i) * zukf(:,i); end Py = 0; Pxy = zeros(12,3); for i = 1 : 24 Py = Py + W(i) * (zukf(:,i) - zhat) * (zukf(:,i) - zhat)'; Pxy = Pxy + W(i) * (xbreve(:,i) - xhatukf) * (zukf(:,i) - zhat)'; end Py = Py + R; Kukf = Pxy/Py; xhatukf = xhatukf + Kukf * (z - zhat); Pukf = Pukf - Kukf * Py * Kukf'; xhatukfArray = [xhatukfArray xhatukf]; Parray = [Parray diag(P)]; Pukfarray = [Pukfarray diag(Pukf)]; resc(k,1:3) = [xhatukf(1:3)-qq2phi(qnb,qnb0)]'; % 滤波估计值 - 计算平台误差 res2(k,1:3)=qq2phi(qnb,qnb0)'; res1(k,1:3) = [xhatukf(1:3)]'; res(k,4:12) = [xhatukf(4:12)]'; pss(k,:)=(diag(Pukf))'; k=k+1; end %% time = [1:k-1]*Ts; close all; figure; subplot(3,2,1); plot(time,res(:,1)/glv.min); ylabel('\it\delta\phi_E\rm / arcmin'); grid on subplot(3,2,3); plot(time,res(:,2)/glv.min); ylabel('\it\delta\phi_N\rm / arcm
评论
    相关推荐
    • GaussDB_100_1.0.1-DATABASE-REDHAT-64bit.tar.gz
      guassdb100在redhat上安装包,单机部署的包,安装步骤请看我的文中介绍,经过大量实验搭建总结出来的文档
    • SIM800C_MQTT.rar
      使用SIM800C模块,使用MQTT协议,连接中国移动onenet平台,能实现数据的订阅、发布、存储等
    • 卷积神经网络
      这是卷积神经网络的一个实际用例,已经调试好了,能够在matlab上成功运行,适合从事卷积神经网络(CNN)研究的人员学习使用。
    • android从bootloader到launcher启动流程整理
      讲述android 开机流程 从boot rom---bootloader---init--zygote---systemserver---ams 并附上自己整理的每个流程的流程图 ,清晰熟悉android 启动流程
    • 分数阶混沌系统:分数阶混沌系统的数值解。-matlab开发
      该工具箱包含可用于模拟一些著名的分数阶混沌系统的函数,例如: - 陈的系统, - Arneodo的系统, - Genesio-Tesi 的系统, - 洛伦兹系统, - 牛顿-莱普尼克系统, - 罗斯勒的系统, - Lotka-Volterra系统, - 达芬的系统, - 范德波尔的振荡器, -伏打的系统- 陆氏系统, - 刘的系统, - Chua的系统, - 金融系统, - 3 细胞 CNN。 这些函数以数值方式计算描述混沌系统的分数阶非线性微分方程的解。 每个函数返回总模拟时间的状态轨迹(吸引器)。 更多详情请看书: Ivo Petras,分数阶非线性系统:建模、分析和仿真,Springer,系列:非线性物理科学,2011,ISBN 978-3-642-18100-9。 http://www.springer.com/engineering/control/book/978-3-
    • matlabpam代码-dsp-library:用于光通信的MATLABDSP函数库
      matlab pam代码 DSP库 该项目包含几种不同的功能,可将DSP算法应用于光通信。 这些功能适用于相干和非相干(PAM,DMT)光通信。 大多数功能是相互独立的。 因此,该代码中的功能可以轻松使用,并与其他DSP功能结合使用。 用法 功能列表以及简短说明在文件中。 输入和输出参数的描述在每个函数的标题中。 讯号 通常,输入(和输出)信号在第一维度上具有时间(例如,列向量),而第二维度用于一次管理多个信号(例如,不同的极化,不同的参数等)。 之所以选择这种约定,是因为MATLAB通过将列保留在内存的连续部分中来存储矩阵,因此,这种约定比其他方法(行向量)要快。 参数 大多数功能使用参数结构作为输入参数。 函数中使用的参数的描述通常在标头中,而coherent-dsp函数的默认参数在文件中。 参考 执照 此代码在下发布。
    • 有关多目标跟踪的PHD滤波的一些资料
      一些关于多目标跟踪的新的文献,主要是有关概率假设密度(PHD)的
    • 基于python开发的全国新工商采集工具 v1.2版本
      这个软件是通过scrapy爬虫框架结合代理IP池再加上request模拟请求技术以及验证码识别技术,可以做到日更新采集全国新工商信息。采集的数据自动存储在mysql数据库表里,可下载全量1.8亿多企业工商基本信息和36维度的详细信息.支持sql和excel导出数据包格式。
    • Aerosim Blockset
      The AeroSim aeronautical simulation blockset provides a complete set of tools for the rapid development of nonlinear 6-degree of freedom aircraft dynamic models. In addition to the basic aircraft dynamics blocks, the library also includes complete aircraft models which can be customized through parameter files.
    • matlab匹配滤波代码-matlab_for_thesis:Matlab博士学位论文代码
      matlab匹配滤波代码博士论文的MATLAB代码 博士论文的MATLAB代码的一部分,“井田双色散水下声通道中的多载波通信”。 函数下的func_JingTian文件夹 FUNC_JINGTIAN包含多载波通信中一些通常需要的功能: OFDM调制/解调模块,包括几种数据辅助的信道估计方法和差分解调; 快速实现GFDM和C-FBMC调制/解调,分别包括时域和频域的迫零(ZF)和匹配滤波(MF)均衡; 为OFDM信道估计中使用的压缩感测方法计算字典的功能; 增加信道效应,施加宽带多普勒失真的功能; 通过线性调频Z变换(CZT)以任意精度对频域中的信号进行重采样; Hermite函数合成的信号之间的交叉歧义函数的计算。 演示文件夹 以下列出了四个演示 demo1:使用基本追踪(BP)算法形式的压缩感知方法对OFDM进行稀疏2-D信道估计; demo2:比较基本数据辅助OFDM信道估计算法,包括常规频域插值和基于IDFT的变换域方法; demo3:圆形滤波器组多载波(C-FBMC / OQAM)的基于DFT特征向量的原型滤波器合成; demo4:用于广义频分复用(GFDM)的辅助日期辅助无干