组合导航系统的计算程序代码
低精度IMU与GPS组合导航系统研究
3、导航数据融合效果有待进一步提高。
3、算法优化：针对卡尔曼滤波算法复杂度较高的问题，采用高效数值计算方 法，优化算法性能，提高实时性。
ห้องสมุดไป่ตู้
3、算法优化：针对卡尔曼滤波 算法复杂度较高的问题
3、算法优化：针对卡尔曼滤波算法复杂度较高的问题，采用高效数 值计算方法，优化算法性能，提高实时性。
1、GPS和IMU数据采集与同步：采用分频复用技术，实现GPS和IMU数据的同 步采集；
3、算法优化：针对卡尔曼滤波算法复杂度较高的问题，采用高效数 值计算方法，优化算法性能，提高实时性。
2、数据预处理：对原始数据进行滤波和平滑处理，以提高数据质量； 3、状态估计：采用扩展卡尔曼滤波算法，估计系统的状态变量和协方差；
3、算法优化：针对卡尔曼滤波算法复杂度较高的问题，采用高效数 值计算方法，优化算法性能，提高实时性。
2、GPS和捷联惯导组合导航系统具有互补性，可以实现优势互补， 提高导航系统的性能。
然而，本研究仍存在一些不足之处。首先，对于GPS和捷联惯导组合导航系统 的具体实现方法，尚未进行详细探讨。未来研究可以进一步深入研究系统的硬件 实现方法、软件算法等具体技术细节。其次，虽然本次演示对GPS和捷联惯导组 合导航系统的应用进行了简要介绍，但尚未对其在各领域的应用进行深入研究。 未来可以对不同领域的应用场景进行详细分析，为实际应用提供更有针对性的指 导。
4、实现卡尔曼滤波算法：根据预处理后的数据和状态估计结果，实现卡尔曼 滤波算法，进行数据融合；
3、算法优化：针对卡尔曼滤波算法复杂度较高的问题，采用高效数 值计算方法，优化算法性能，提高实时性。
5、系统调试与优化：对系统进行实际环境下的调试与优化，确保系统的稳定 性和性能。
Python语言下的智能车载导航系统设计与实现
Python语言下的智能车载导航系统设计与实现随着科技的不断发展，智能车载导航系统已经成为现代汽车的标配之一。
而Python作为一种功能强大且易于学习的编程语言，被广泛应用于各种领域，包括车载导航系统的设计与实现。
本文将介绍如何利用Python语言来开发智能车载导航系统，包括系统架构设计、地图数据处理、路径规划算法、实时导航功能等方面。
1. 系统架构设计智能车载导航系统的核心功能包括地图显示、路径规划、实时导航等。
在Python语言下，可以采用模块化的设计思路来构建系统架构。
主要模块包括地图模块、导航模块、用户界面模块等。
地图模块负责地图数据的加载和显示，导航模块实现路径规划算法和实时导航功能，用户界面模块提供友好的交互界面。
2. 地图数据处理在智能车载导航系统中，地图数据是至关重要的。
Python语言提供了丰富的库和工具来处理各种类型的地图数据，包括矢量地图、栅格地图等。
可以利用开源地图数据或者商业地图数据来构建系统所需的地图数据库，并通过Python程序进行读取和解析。
3. 路径规划算法路径规划是智能车载导航系统中的核心功能之一。
常用的路径规划算法包括Dijkstra算法、A*算法、最短路径树算法等。
在Python语言下，可以借助第三方库如NetworkX来实现这些算法，并结合地图数据进行路径规划。
4. 实时导航功能实时导航是智能车载导航系统中用户最为关注的功能之一。
通过GPS定位和传感器数据，可以实现车辆位置的实时更新，并结合路径规划算法提供实时导航指引。
Python语言提供了丰富的库和工具来处理实时数据流，可以轻松实现实时导航功能。
5. 用户界面设计用户界面是智能车载导航系统中用户与系统交互的窗口。
在Python语言下，可以利用Tkinter、PyQt等GUI库来设计用户界面，包括地图显示、路径规划结果展示、语音提示等功能。
通过简洁直观的用户界面，提升用户体验。
6. 总结通过本文对Python语言下智能车载导航系统设计与实现的介绍，我们了解到了系统架构设计、地图数据处理、路径规划算法、实时导航功能以及用户界面设计等方面的重要内容。
KY-INS112 组合导航系统 使用说明书
KY-INS112组合导航系统使用说明书北京北斗星通导航技术股份有限公司导航产品事业部目录1.概述 (1)2.功能及指标 (1)2.1主要功能 (1)2.2性能指标 (1)3.工作原理 (3)3.1.产品组成 (3)3.2.基本原理 (3)4.使用说明 (4)4.1外形尺寸 (4)4.2电气接口 (5)5.系统导航工作流程 (8)5.1.组合导航流程 (8)5.2.纯惯性导航流程 (8)6.产品配置 (9)6.1.设备接口功能 (9)6.2.配置查询 (10)6.3.波特率配置 (10)6.4.协议及更新率配置 (10)6.5.初始值配置 (12)6.6.功能模块配置 (12)6.7.“零速修正”配置 (12)6.8.“位置输出平滑”配置 (13)6.9.载体类型配置 (13)6.10.GNSS天线杆臂配置 (14)6.11.输出杆臂设置 (15)6.12.安装角设置 (15)6.13.输出角设置 (16)6.14.强制转惯性导航 (16)6.15.系统复位 (17)7.输出语句解析格式 (17)7.1.可输出的协议类型 (17)8.存储数据导出 (22)9.系统维护 (24)9.1.固件升级 (24)9.2.参数上传 (24)10.注意事项 (25)11.附录 (25)11.1.卫星接收机COM2输出配置 (25)11.2.差分配置说明 (26)11.2.1.差分基准站设置 (27)11.2.2.差分通讯链路设置 (27)11.2.3.差分移动站设置 (28)11.3.32位CRC校验计算方法 (28)1. 概述KY-INS112组合导航系统由MEMS传感器及高端GNSS 接收机板卡（NovAtel-718D ）组成，通过多传感器融合及导航解算算法实现。
该产品可靠性高，环境适应性强。
通过匹配不同的软件，产品可广泛应用于无人机、无人车、测绘、船用罗经、稳定平台、水下运载器等领域。
2. 功能及指标2.1主要功能组合导航系统能够利用GNSS 接收机接收到的卫星导航信息进行组合导航，输出载体的俯仰、横滚、航向、位置、速度、时间等信息；失去信号后输出惯性解算的位置、速度和航姿信息，短时间内具备一定的导航精度保持功能。
多卫星导航系统组合定位解算
测控 遥感 与 导航 定 位
多卫 星导 航 系统 组 合 定位 解 算
车 斐
（ 中国 电子科 技集 团公 司第 ５ ４研 究所 ， 河北 石 家庄 ０ ０ ８ ） ５０ １
摘 要 主要 研 究 由 Ｇ Ｓ Ｇ ｌｅ Ｐ 、ａｌ ｉｏ和北 斗 等 多 个卫 星 导航 系 统 互 相 辅 助 ， 现 组 合 定 位解 算 。 由于 各 独 立 卫 星 导 航 定 位 实
和连 续 性 。
关 键 词 北 斗双 星 定 位 系 统 ；Ｐ ； 合 定 位 算法 ； 位精 度 ＧＳ组 定 中图 分 类 号 Ｔ ９７ １ Ｎ６． 文献 标 识 码 Ａ 文 章编 号 １０ —３０ （０７０ —０３ ０ ０ ３ １６ ２０ ）３ ０４— ２
Ｉ ｔ ｇ ａ ｅ ｓｔｏ ｉ ｇ Ａｌ ｏ ｉｈ ｉ ｇ Ｍ ｕ ｔｐ ｅ ｔ ｌ ｔ ｎ ｅ ｒ ｔ ｄ Ｐｏ ｉｉ ｎ ｎ ｇ ｒｔ ｍ Ｕｓｎ ｌｉ ｌ ａ ｅｌ ｅ ｉ Ｎａ ｉ ａ ｉ ｎ ａ ｄ Ｐｏ ｉ ｏ ｎ ｙ ｔ ｍ ｓ ｖ ｇ ｔｏ ｎ ｓｔ ｎ ｇ Ｓ ｓｅ ｉｉ
Ｃ ｅ ＨＥ Ｆ ｉ
（ ｈ ４ｈ Ｒ ｓ ｒ ｓｔ ｅｏ Ｅ Ｃ， ｈ ｉ ｈ ａｇ Ｈ ｂ ５ ０ １ Ｃ ｉ ） Ｔｅ５ ｔ ｅ ａ ｈＩ ｔ ｔ ｆＣ Ｔ Ｓ ｉ ｚｕ ｎ ｅｅ ０ ０ ８ ， ｈｎ ｅ ｃ ｎｉ ｕ ｊ ａ ａ
Ａｂ ｔａ ｔ Ｔ ｓｐ ｐ ｒｆｃ ｓ ｓｏ ｈ ｎ ｅ ｒｔｄ ｐ ｓｔｏ ｎ ｌｏｉｈ ｕ ｉｇ ＧＰ Ｇａｉｅ ｎ ｓｒ ｃ ｈｉ ａ ｅ ｏ ｕｅ ｎ ｔｅ ｉｔｇ ａｅ ｏ ｉｉｎｉｇ ａｇ ｒｔｍ ｓｎ Ｓ， ｌ ｏ ａ ｄ ＢＤ 一 １ ａｅｌｔ ｏ ｉｏ ｉｇ ｓ ｓｅ ， ｌ ｓｔｌｉ ｐ ｓｔ ｎｎ ｙ ｔｍｓ ｗｈｉｈ ｅ ｉ ｃ
GPS与惯导系统的组合导航技术
谢谢观看
LOGO
GPS/INS
INS:
INS 不仅能够提供载体位置、速度参数,还能提 供载体的三维姿态参数,是完全自主的导航方式,在 航空、航天、航海和陆地等几乎所有领域中都得 到了广泛应用。但是,INS 难以克服的缺点是其导航 定位误差随时间累加,难以长时间独立工作。
LOGO
GPS/INS
GPS/INS组合:
LOGO
紧耦合和松耦合
优点:
1.组合结构简单，便于工程实现，便于实现容错 2.两个系统能够独立工作，使得导航系统有一定的 余度
缺点:
1. GPS 输出的位置、速度通常是与时间相关的； 2.INS 和 GPS 信息流动是单向的，INS 无法辅GPS。
LOGO
GPS/INS
紧耦合:
紧耦合模式是指利用 GPS 接收机的的原始信息来和惯 导系统组合，原始信息一般是指伪距、伪距率、载波 相位等。
LOGO
分类:
基于卡尔曼组合数据的融合方法
按照组合中滤波器的设置来分类，可以分成： 集中式的卡尔曼滤波 分布式的卡尔曼滤波 按照对系统校正方法的不同，分为: 开环校正(输出校正) 闭环校正(反馈矫正) 按照组合水平的深度不同，分为： 松耦合 紧耦合 根据卡尔曼滤波器所估计的状态不同，卡尔曼 滤波在组合导航中的应用有: 直接法 间接法
目录
2 3
LOGO
紧耦合和松耦合
基于卡尔曼滤波的组合方式:
利用卡尔曼滤波器设计 GPS/INS 组合导航系统的方法 多种多样按照组合水平的深度不同，分为： 松耦合 紧耦合
LOGO
紧耦合和松耦合
松耦合:
松耦合模式是指直接利用 GPS 接收机输出的定位信 息与 INS 组合，它是一种 低水平的组合。位置、速 度组合是其典型代表，它 采用 GPS 和 INS 输出的位 置和速度信息的差值作为 量测值。
组合导航算法设计
INS/GPS组合导航算法设计1 引言目前单一导航系统难以满足实际要求，把两种或多种导航系统组合起来，应用最优估计理论，形成最优组合导航系统，使组合后的导航系统在精度和可靠性都有所提高。
本课题研究飞行器GPS/INS组合导航算法，通过对飞行器INS/GPS 组合导航算法设计，以VC++6.0为平台组建INS/GPS组合导航仿真系统，对组合导航算法进行实现。
并对飞行器的飞行状态进行仿真，仿真前预先设定飞行器的飞行参数（包括平飞、加速、减速、上升、下降、转弯等飞行动作以及每个动作开始结束的时间），通过设计的组合导航仿真系统得到飞行器的位置、速度、姿态角信息。
并通过MATLAB对INS/GPS组合导航解算出来的数据与预先设定的实际飞行数据进行比较分析。
惯性导航系统的优点是：（1）自主性强，它可以不依赖任何外界系统得支持，单独进行导航。
（2）不受环境、载体运动和无线电干扰的影响，可连续输出包括基准在内的全部导航参数，实时导航数据更新率高。
（3）具备很好的短期精度和稳定性。
其主要缺点是导航定位误差随时间增长，难以长时间的独立工作。
全球定位系统是一种高精度的全球三维实时导航的卫星导航系统，其导航定位的全球性、高精度、误差不随实践积累的优点，但是GPS系统也存在一些不足之处，主要是：GPS接收机的工作受飞行机动影响，当载体的机动超过GPS接收机的动态范围时，GPS接收机会失锁，从而不能工作，或者动态误差过大，超过允许值，不能使用。
且GPS接收机的更新频率较低（1HZ），难以满足实时控制的要求。
抗干扰能力差。
此外GPS导航受制于人。
因此GPS系统一般作为理想的辅助导航设备使用。
GPS/惯性组合导航，克服了各自的缺点，取长补短，可以构成一个比较理想的导航系统，GPS/惯性组合导航可以大大降低导航系统的成本。
随着MEMS技术的发展，惯导成本的降低都是组合导航系统发展的优势所在。
我们用组合导航算法将惯性导航单元的信息和GPS的信息进行综合，来补偿惯性元件的误差，修正速度、姿态信号，从而构成一个精度适中、结构紧凑、成本低廉的导航系统。
C语言实现导航功能
C语⾔实现导航功能本⽂实例为⼤家分享了C语⾔实现导航功能的具体代码，供⼤家参考，具体内容如下#include<stdio.h>#include<string.h>#define NUM 25#define INFINITY 32767#define False 0#define True 1typedef struct{int number;//顶点的编号const char *sight;//顶点的信息} VertexType;//顶点的类型typedef struct{VertexType vex[NUM];//存放顶点信息int arcs[NUM][NUM];//邻接矩阵数组int vexnum;//顶点个数}MGraph;MGraph G;/**由传⼊的节点个数创建图**/void GreateMGraph(int v){G.vexnum=v;//传⼊节点个数for(int i=1;i<G.vexnum;i++){G.vex[i].number=i;}//配置顶点编号/**编辑顶点信息**/G.vex[0].sight="各景点名字";G.vex[1].sight="⼤门⼝";G.vex[2].sight="⾏政办公楼";G.vex[3].sight="北区教室实训中⼼";G.vex[4].sight="⼀号教学楼";G.vex[5].sight="⼆号教学楼";G.vex[6].sight="实验楼";G.vex[7].sight="三号教学楼";G.vex[8].sight="图书馆";G.vex[9].sight="开⽔房";G.vex[10].sight="超市";G.vex[11].sight="榴馨苑";G.vex[12].sight="洗浴中⼼";G.vex[13].sight="骊秀苑";G.vex[14].sight="综合楼";G.vex[15].sight="游泳池";G.vex[16].sight="主⽥径场";G.vex[17].sight="综合⽂体馆";/**先将所有顶点之间的距离设置为INFINITY**/for(int i=1;i<=G.vexnum;i++){for(int j=1;j<=G.vexnum;j++){G.arcs[i][j]=INFINITY;}}/**设置各顶点之间的距离**/G.arcs[1][2]=G.arcs[2][1]=255;G.arcs[1][4]=G.arcs[4][1]=501;G.arcs[1][5]=G.arcs[5][1]=535;G.arcs[1][6]=G.arcs[6][1]=705;G.arcs[1][7]=G.arcs[7][1]=722;G.arcs[1][8]=G.arcs[8][1]=790;G.arcs[2][3]=G.arcs[3][2]=530;G.arcs[2][4]=G.arcs[4][2]=450;G.arcs[2][5]=G.arcs[5][2]=484;G.arcs[2][6]=G.arcs[6][2]=654;G.arcs[2][7]=G.arcs[7][2]=663;G.arcs[2][8]=G.arcs[8][2]=748;G.arcs[3][8]=G.arcs[8][3]=1054;G.arcs[3][17]=G.arcs[17][3]=713;G.arcs[4][5]=G.arcs[5][4]=436;G.arcs[4][6]=G.arcs[6][4]=158;G.arcs[4][7]=G.arcs[7][4]=527;G.arcs[4][8]=G.arcs[8][4]=534;G.arcs[5][6]=G.arcs[6][5]=688;G.arcs[5][7]=G.arcs[7][5]=561;G.arcs[5][8]=G.arcs[8][5]=603;G.arcs[6][7]=G.arcs[7][6]=428;G.arcs[6][8]=G.arcs[8][6]=329;G.arcs[6][9]=G.arcs[9][6]=547;G.arcs[7][8]=G.arcs[8][7]=254;G.arcs[8][11]=G.arcs[11][8]=421;G.arcs[8][17]=G.arcs[17][8]=879;G.arcs[9][10]=G.arcs[10][9]=178;G.arcs[10][11]=G.arcs[11][10]=213;G.arcs[10][12]=G.arcs[12][10]=114;G.arcs[12][13]=G.arcs[13][12]=415;G.arcs[13][14]=G.arcs[14][13]=104;G.arcs[13][16]=G.arcs[16][13]=427;G.arcs[13][15]=G.arcs[15][13]=576;G.arcs[14][17]=G.arcs[17][14]=688;G.arcs[15][16]=G.arcs[16][15]=213;G.arcs[16][17]=G.arcs[17][16]=214;}/**展⽰校园地图**/void Map(){printf("\n\n\n");printf(" **************************河南财经政法⼤学*******************************"); printf("\n\n\n");printf(" ------------------------15游泳池 \n");printf(" | | \n");printf(" | | \n");printf(" 12洗浴中⼼----------------13骊绣苑---------------------16主⽥径场 \n");printf(" | | | \n");printf(" 10超市----11榴馨苑 14综合楼 | \n");printf(" | | |----------------------17综合⽂体馆 \n");printf(" 9开⽔房 | | \n");printf(" | ------------8图书馆--------------------------| \n");printf(" | | | \n");printf(" |-------------6实验楼------|--------7三号教学楼 | \n");printf(" | | | | \n");printf(" | | | | \n");printf(" 4⼀号教学楼------|--------5⼆号教学楼 | \n");printf(" | | \n");printf(" | | \n");printf(" |---2⾏政楼---------------3北区 \n");printf(" | \n");printf(" | \n");printf(" 1⼤门⼝ \n");}/**介绍校园各景点概况**/void Info(int sight_num,char data[][200]){if(sight_num==1)puts(data[1]);if(sight_num==2)puts(data[2]);if(sight_num==3)puts(data[3]);if(sight_num==4)puts(data[4]);if(sight_num==5)puts(data[5]);if(sight_num==6)puts(data[6]);if(sight_num==7)puts(data[7]);if(sight_num==8)puts(data[8]);if(sight_num==9)puts(data[9]);if(sight_num==10)puts(data[10]);if(sight_num==11)puts(data[11]);if(sight_num==12)puts(data[12]);if(sight_num==13)puts(data[13]);if(sight_num==14)puts(data[14]);if(sight_num==15)puts(data[15]);if(sight_num==16)puts(data[16]);if(sight_num==17)puts(data[17]);if(sight_num==18)puts(data[18]);if(sight_num==19)puts(data[19]);if(sight_num==20)puts(data[20]);if(sight_num==21)puts(data[21]);if(sight_num==22)puts(data[22]);if(sight_num==23)puts(data[23]);if(sight_num==24)puts(data[24]);if(sight_num==25)puts(data[25]);}/**开始菜单**/int Menu(){int c;Map();printf("\t\t欢迎使⽤河南财经政法⼤学导航图系统\n");printf("\t\t 1.查询地点路径 \n");printf("\t\t 2.地点信息简介 \n");printf("\t\t 3.退出 \n");printf(" **************************河南财经政法⼤学*******************************\n");printf("请输⼊您的选择：");scanf("%d",&c);return c;}/**地图的导航功能**//**输出任意两点之间的最短路径**/void guide_Dispath_two(MGraph g,int dist[],int path[],int S[],int v,int i)//v为起点，i为终点 {int apath[NUM],d=0; //存放⼀条最短的路径以及顶点个数（路径中终点为⾸） int j,k; //k⽤来存放终点的前⾯的节点if(S[i]==1 && i!=v){printf("从顶点%d到顶点%d的路径长度为：%d\t路径为：",v,i,dist[i]);apath[d]=i; //把终点放在数组中的⾸位k=path[i];if(k==-1)printf("⽆路径");/**利⽤循环将最短路径中的各节点存⼊apath数组**/else{while(k!=v){d++;apath[d]=k;k=path[k];}}d++; apath[d]=v; //将起点添加进去printf("%d",apath[d]); //输出起点for(j=d-1;j>=0;j--){printf("->%d",apath[j]); //循环输出最短路径中的各节点}}}//以编号为v的顶点为起点,w为终点void guide_Dijkstra(MGraph g,int v,int w){int dist[NUM],path[NUM];int S[NUM]; //S[i]=1表⽰顶点i在S中，S[i]=0表⽰顶点i在U中int MINdis,i,j,u;for(i=1;i<=g.vexnum;i++){dist[i]=g.arcs[v][i]; //距离初始化（距顶点v的距离）S[i]=0; //S[]置空if(g.arcs[v][i]<INFINITY) //路径初始化path[i]=v; //顶点v到顶点i有边时，置顶点i的前⼀个顶点为顶点velsepath[i]=-1; //顶点v到顶点i没边时，置顶点i的前⼀个顶点为-1}S[v]=1;path[v]=0; //源点编号v放⼊S中for(i=1;i<=g.vexnum-1;i++) //循环直到所有顶点的最短路径都求出{MINdis=INFINITY; //MINdis置最⼤长度初值for(j=1;j<=g.vexnum;j++) //选取不在S中（即U中）且具有最⼩最短路径长度的顶点u {if(S[j]==0 && dist[j]<MINdis){u=j;MINdis=dist[j];}}S[u]=1; //顶点u加⼊S中for(j=1;j<=g.vexnum;j++) //修改不在S中（即U中）的顶点的最短路径{if(S[j]==0)if(g.arcs[u][j]<INFINITY && dist[u]+g.arcs[u][j]<dist[j]){dist[j]=dist[u]+g.arcs[u][j];path[j]=u;}}}guide_Dispath_two(g,dist,path,S,v,w); //输出最短路径}/**将⽂件中的景点信息载⼊数组**/void load_sight_data(char data[][200],MGraph g)FILE *fp;int i;if((fp=fopen("C:\\Users\\admin\\Desktop\\导航\\sight_data.txt","r"))==NULL){printf("File can't open");return;}for(i=1;i<=g.vexnum;i++){fgets(data[i],200,fp);}fclose(fp);}/**修改景点信息**/void change_sight_data(char user_change_data[200],MGraph g){FILE *fin,*ftp;int i;fin=fopen("C:\\Users\\admin\\Desktop\\导航\\sight_data.txt","r");//读打开原⽂件ftp=fopen("C:\\Users\\admin\\Desktop\\导航\\temp.txt","w");//写打开临时⽂件if(fin==NULL || ftp==NULL){printf("打开⽂件失败");return;}for(i=1;i<=g.vexnum;i++){char change_data[200];fgets(change_data,200,fin);if(change_data[0]==user_change_data[0] && change_data[1]==user_change_data[1]){fputs(user_change_data,ftp);//⽽⽤fputs直接将user_change_data直接写⼊⽂件没有换⾏符，需要添加换⾏符 fprintf(ftp,"\n");}else{fputs(change_data,ftp);//在使⽤fgets函数得到change_data数组时换⾏符会被保存，此处不需要加换⾏符}}fclose(fin);fclose(ftp);remove("C:\\Users\\admin\\Desktop\\导航\\sight_data.txt");rename("C:\\Users\\admin\\Desktop\\导航\\temp.txt","C:\\Users\\admin\\Desktop\\导航\\sight_data.txt");}/**增添景点到⽂件内**/void add_sight_data(char change_data[200],MGraph g){FILE *fp;if((fp=fopen("C:\\Users\\admin\\Desktop\\导航\\sight_data.txt","a"))==NULL){printf("File can't open");return;}fprintf(fp,"\n");fputs(change_data,fp);fprintf(fp,"\n");rewind(fp);fclose(fp);}/**修改密码，保存到密码⽂件中**/void change_password(char password[30]){FILE *fp;fp=fopen("C:\\Users\\admin\\Desktop\\导航\\password_temp.txt","w");if(fp==NULL){printf("打开⽂件失败");return;}fputs(password,fp);fclose(fp);remove("C:\\Users\\admin\\Desktop\\导航\\password.txt");rename("C:\\Users\\admin\\Desktop\\导航\\password_temp.txt","C:\\Users\\admin\\Desktop\\导航\\password.txt"); }/**将密码装进密码数组中**/void load_password(char password[30]){FILE *fp;if((fp=fopen("C:\\Users\\admin\\Desktop\\导航\\password.txt","r"))==NULL){printf("File can't open");return;}fgets(password,30,fp);fclose(fp);}int admin_Menu(){int c;printf("**************管理系统****************\n");printf("\t\t欢迎使⽤管理员操作系统\n");printf("1.修改登录密码\n");printf("2.添加新景点\n");printf("3.修改景点信息\n");printf("4.新建景点路径\n");printf("5.退出\n");printf("**************************************\n");printf("请输⼊您的选择：");scanf("%d",&c);return c;}int main(){GreateMGraph(17);char sight_data[NUM][200];char password[30];int choice;int Menu_choice;int start,end;do{printf("欢迎使⽤河南财经政法⼤学导航系统\n");printf("请问您的⾝份是：1.管理员 2.游客 3.退出\n");scanf("%d",&choice);if(choice==2){do{Menu_choice=Menu();if(Menu_choice==1){printf("请输⼊您现在的位置：");scanf("%d",&start);printf("\n");printf("请输⼊您想要到达的位置：");scanf("%d",&end);guide_Dijkstra(G,start,end);}if(Menu_choice==2){int sight_num;load_sight_data(sight_data,G);printf("请输⼊您要查询的景点编号：");scanf("%d",&sight_num);Info(sight_num,sight_data);}if(Menu_choice==3){break;}}while(1);}if(choice==1){getchar();load_password(password);char user_input_psw[30];printf("请输⼊管理员登录密码：");gets(user_input_psw);if(strcmp(password,user_input_psw)==0){printf("密码正确！\n");int admin_choice;do{admin_choice=admin_Menu();if(admin_choice==1){getchar();char user_change_psw[30];printf("请输⼊新的密码：\n");gets(user_change_psw);change_password(user_change_psw);load_password(password);printf("密码修改成功！\n");}if(admin_choice==2){getchar();char uadd_sight_data[200];printf("请输⼊您要添加的景点以及该景点信息：\n"); gets(uadd_sight_data);add_sight_data(uadd_sight_data,G);load_sight_data(sight_data,G);G.vexnum++;char *p;p=strtok(uadd_sight_data," ");p=strtok(NULL," ");G.vex[G.vexnum].number=G.vexnum;G.vex[G.vexnum].sight=p;printf("添加成功！\n");}if(admin_choice==3){getchar();char user_sight_data[200];printf("请输⼊您要修改的景点信息：\n");gets(user_sight_data);change_sight_data(user_sight_data,G);load_sight_data(sight_data,G);printf("修改成功！\n");}if(admin_choice==4){int new_start,new_end;int length;printf("请输⼊您想要在哪两点之间添加路线：\n"); printf("起点：");scanf("%d",&new_start);printf("\n");printf("终点：");scanf("%d",&new_end);printf("请输⼊两顶点之间的距离：\n");scanf("%d",&length);printf("\n");G.arcs[new_start][new_end]=G.arcs[new_end][new_start]=length;printf("路线添加成功！\n");}if(admin_choice==5){break;}}while(1);}if(strcmp(password,user_input_psw)!=0){printf("密码错误! \n");}}if(choice==3){break;}}while(1);return 0;}password.txt⽂件⽤来存放密码sight_data.txt⽂件⽤来存放景点信息：1 ⼤门⼝出⼊学校的必经之路2 ⾏政办公楼学校最⽓派的建筑之⼀3 北区⾦⼯实训中⼼，还有⼏排具有历史沧桑感的教室4 ⼀号教学楼主要有⼩教室，⽤来上英语课和专业课5 ⼆号教学楼主要⽤来上专业课，五六楼有语⾳室6 实验楼学⽣上各种实验课的地点7 三号教学楼有⼤教室，⼀般安排⽤来上基础课8 图书馆学校为同学们提供学习和⾃习的地⽅，也是学校的藏书最多的地⽅9 开⽔房学校唯⼀⼀个为同学提供热⽔的地点10 超市学校唯⼀⼀个中型超市，在这⾥可以买到各种⽣活⽤品11 榴馨苑环境较好的学⽣⾷堂，这⾥因为离⼥⽣公寓较近，所以这个⾷堂⼥⽣较多12 洗浴中⼼环境还⾏就是规模太⼩，每天都是供不应求13 骊秀苑主要经营⾯⾷。
C语言在智能车载导航系统中的优化与实现
C语言在智能车载导航系统中的优化与实现智能车载导航系统是现代汽车领域中的重要组成部分，它通过结合地图数据和实时交通信息，为驾驶员提供最佳的行车路线规划和导航指引。
在智能车载导航系统的开发过程中，C语言作为一种高效、灵活的编程语言，扮演着至关重要的角色。
本文将探讨C语言在智能车载导航系统中的优化与实现。
1. 智能车载导航系统的基本原理智能车载导航系统主要由地图数据处理模块、路线规划模块、导航指引模块等组成。
其中，地图数据处理模块负责加载和解析地图数据，路线规划模块根据起点和终点位置计算最佳行车路线，导航指引模块则向驾驶员提供语音提示和地图显示。
2. C语言在智能车载导航系统中的优势C语言作为一种底层编程语言，具有高效、灵活、可移植等特点，非常适合用于开发智能车载导航系统。
其优势主要体现在以下几个方面：性能优越：C语言编写的程序执行效率高，可以更快速地处理大规模地图数据和复杂算法。
直接操作硬件：C语言可以直接操作硬件资源，与底层设备进行交互，保证系统的稳定性和可靠性。
丰富的库支持：C语言拥有丰富的标准库和第三方库，可以方便地实现各种功能模块。
跨平台性：C语言编写的程序具有较好的跨平台性，可以在不同硬件平台上运行。
3. C语言在智能车载导航系统中的应用3.1 地图数据处理在智能车载导航系统中，地图数据处理是一个关键环节。
C语言可以通过文件操作函数读取地图数据文件，并进行解析和存储。
同时，利用C语言的数据结构和算法知识，可以高效地构建地图索引结构，加快路线规划的速度。
3.2 路线规划算法路线规划是智能车载导航系统中的核心功能之一。
C语言可以实现各种路线规划算法，如Dijkstra算法、A*算法等。
通过合理选择算法和优化代码逻辑，可以提高路线规划的准确性和效率。
3.3 导航指引功能导航指引功能需要实时响应驾驶员的操作，并提供准确的导航信息。
借助C语言多线程编程技术，可以实现多任务并发处理，保证导航指引功能的及时性和流畅性。
导航代码全
导航代码全/* 第一部分、静态背景颜色*//* 1、首页/店铺动态/其它导航类目的背景色，这里设为红色*/.skin-box-bd .menu-list .link{background:red;}/* 2、所有分类的背景色（最左边的），一样设为红色*/.all-cats .link{background:red;}/* 到这里，发觉右边的颜色还没有变呢，好的，接着下一步*//* 3、导航条整个分类段背景色，还是要设为红色，整体布局好看些*/.skin-box-bd .menu-list{background:red;}/* 注意观察，最右边还有一丝地方没有变*//* 4、导航条背景色（是最底层了吧），修补导航右侧缺口，再设为红色*/.skin-box-bd{background:red;}/* 小结：有点成功感了！换换其它颜色试试看吧*//* 背景色最好搭配页头背景图，才能整体大气美观*//* 第二部分、分隔线、静态文字的颜色*//* 5、首页等分类的右边的分隔线颜色，设为白色*/ .menu-list .menu{border-color:white;}/* 6、所有分类的右边的分隔线颜色，设为白色*/ .all-cats .link{border-color:white;}/* 7、首页/店铺动态/其它导航类目的文字颜色*/ .menu-list .menu .title{color:yellow;}/* 8、所有分类的文字颜色（最左边那个）,*/.all-cats .link .title{color:yellow;}/* 小结：其实原来默认文字的颜色也挺好的*//* 第三部分、分类下的颜色*//* 9、二级分类的背景色，设为灰色*/.popup-content{background:gray;}/* 10、三级分类的背景色，我设为深灰色*/.popup-content .cats-tree .snd-pop-inner{background:#504f4f;}/* 11、二级分类的文字颜色，设为黄色*/.popup-content .cat-name{color:yellow;}/* 12、三级分类的文字颜色。
导航与制导实验报告INS与GPS位置组合导航
导航与制导原理实验报告一．实验要求1.完成INS与GPS位置组合导航的仿真;2.画出组合导航后的位置误差、速度误差曲线;3.画出原始轨迹与组合导航后的轨迹比较图；（画图时弧度制单位要转换成度分秒制单位）4.结果分析5.提交纸版实验报告（附上代码）二．全局变量R=6378160; %地球半径（长半轴）f=1/298.3; %地球扁率wie=7.2921151467e-5; %地球自转角速率g0=9.7803267714; %重力加速度基础值deg=π/180; %角度min=deg/60; %角分sec=min/60; %角秒hur=3600; %小时dph=deg/hur; %度/时ts=0.1; %仿真采样时间三．组合导航仿真变量GPS_Sample_Rate=10; %GPS采样时间Runs=10; %由于随机误差，使用Kalman滤波时，应多次滤波，以求平均值Tg = 3600; %陀螺仪Markov过程相关时间Ta = 1800; %加速度计Markov过程相关时间四．Kalman Filter:估计状态初始值：Xk = zeros(18,1);估计协方差初始值：Pk=diag([min,min,min,0.5,0.5,0.5,30/Re,30/Re,30, 0.1*dph, 0.1*dph, 0.1*dph, 0.1*dph,0.1*dph, 0.1*dph, 1.e-3,1.e-3,1.e-3].^2); %18*18矩阵系统噪声方差：Qk=1e-6*diag([0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.9780]).^2量测噪声方差：Rk=diag([1e-5,1e-5,10.3986]).^2系数矩阵 F， G， H 的表示，参考课件 6.2.1。
五．可能用到的公式(1)四元数Q的即时修正（符号⨂表示四元数乘法）式中nb nbx nby nbz 0Tbb b b ⎡⎤ω=ωωω⎦⎣为向量扩展四元数，标量部分为0。
组合导航算法总结
组合导航算法总结引言组合导航是一种将多个导航算法相结合的技术，旨在提高导航系统的准确性和效率。
该算法通过采用多个不同的导航策略，并将它们的结果进行组合来取得更好的导航性能。
本文将对组合导航算法进行总结，并介绍其应用场景和优势。
组合导航算法的原理组合导航算法的核心原理是通过将多个独立的导航算法的结果进行组合，从而得到更准确和可靠的导航路径。
通常情况下，组合导航算法会采用多种导航策略，如最短路径算法、遗传算法、模拟退火算法等。
这些不同的导航策略可能会有不同的优势和局限性，通过将它们组合起来，可以克服各自的限制，提高导航性能。
组合导航算法的应用场景组合导航算法可以应用于各种导航系统，比如车载导航、无人机导航、船舶导航等。
在这些场景中，导航的准确性和效率对于任务的完成至关重要。
而组合导航算法可以通过融合多种导航策略的结果，来提供更可靠和精确的导航路径。
组合导航算法的优势使用组合导航算法的主要优势包括以下几点：1.提高导航准确性：组合导航算法可以充分利用多个导航策略的优势，从而减小导航误差，提高导航准确性。
2.提高导航鲁棒性：由于组合导航算法考虑了多种导航策略，即使某个策略无法适应特定的导航场景，其他策略仍然可以提供可行的导航路径，从而提高导航的鲁棒性。
3.提高导航效率：组合导航算法可以通过并行计算多个导航策略，从而加快导航路径的计算和更新速度，提高导航效率。
4.适应多样化的导航环境：由于组合导航算法可以灵活选择不同的导航策略，因此可以适应各种不同的导航环境和场景，包括复杂的城市道路、山区、海洋等。
总结组合导航算法是一种将多个导航策略相结合的技术，可以提高导航系统的准确性和效率。
通过使用组合导航算法，可以充分利用多种导航策略的优势，克服各自的限制，从而获得更可靠、精确和高效的导航路径。
因此，组合导航算法在各种导航系统中有着广泛的应用前景。
注意：本文仅为组合导航算法总结，若需详细了解该算法的具体实现和应用细节，请查阅相关专业文献或与领域专家进行交流。
船舶驾驶台组合导航仪使用说明
船舶驾驶台组合导航仪使用说明ATLAS组合导航仪是一个完整的导航系统，它能使船舶安全，经济地航行在计划航线上，并且按照计划速度计算出每个转向点及到达点的ETA，也可以输入ETA计算出所要求的速度.它和船上所有重要的航海仪器相连接，如GPS、罗经、测深仪、多普勒计程仪、舵、气象站等，在NCC显示器上方便地显示出这些航行要素，从而确保整个航次的航行安全.它的显著特点是集雷达、ARPA、导航系统于一体，从而实现单人驾驶的目的.NACOS由以下单元组成:9600型雷达1台(RADARPILOT)，9800型雷达1台(MULTIPILOT)，导航控制监视器(NCC)，设计咨询站(CHART PILOT)，自适应导航电子单元，多功能导航电子单元.一. 9800型雷达(MULTIPILOT)9800型雷达由MULTIPILOT监视器，MULTIPILOT操作单元，雷达收发机单元及天线组成.1.MULTIPILOT监视器在RADAR显示模式时分为五个数据显示区域.1.1. D1区域显示:(1)雷达距离档;(2)固定距标的开与关;(3)雷达显示模式;(4)频率波段; (5)脉冲长短;(6)天线;(7)真运动或相对运动显示.1.2. D2区域显示:(1)清除扫描线的杂波;(2)同频干扰;(3)海浪干扰抑制;(4)雨水干扰抑制;(5)调谐及自动调谐;(6)辅助雷达;(7)蜂鸣器警报符号.1.3. D3区域显示:(1)马克光标的数据[离扫描中心的距离与方位];(2)马克标的经纬度数据.1.4. D4区域显示:(1)PCS[计划咨询站]的状态;(2)船艏线弯曲显示状态;(3)活动距标数据;(4)电子方位线数据.1.5. D5区域显示:(1)速度显示;(2)导航数据;(3)船位;(4)航线和海图[MAP];(5)信息内容[包括各种警报];(6)其它信息[如菜单内容，捕捉目标数据，装载的计划航线内容等].2. MULTIPILOT操作单元在面板上分五个区域2.1 雷达基本功能键区域；(1)STD BY ON:预备键，当开启雷达电源后，等上3分钟，按此键，即可使雷达进入工作状态；(2)MAP ADJUST:调整雷达图像(只能在推算船位时使用)；(3)CENT/OFF:电子方位线开关，或将电子方位线回至中心；(4)EBM1/EBM2:选择电子方位线1或2；(5)FIX REL:相对电子方位线；(6)FIX ABS:绝对电子方位线；(7)REL:相对运动显示(可用于看来船过本船头或本船尾)；(8)RANGE:调整雷达量程；(9)DAY:亮度调整(白天使用)；(10)NIGHT:亮度调整(晚上使用)；(11)CLEAR VIDEO:雷达图像回波抑制；(12)CLEAR SYNTH:船艏线及计划航线，电子海图抑制；(13)OFF CENTER:分SET和RESET(即偏心显示和复原)；(14)TUNING:调谐旋钮；(15)GAIN:增益旋钮；(16)FTC:雨雪干扰抑制；(17)STC:海浪干扰抑制；(18)VRM:活动距标；(19)DO:确认键；(20)INFO:信息键；(21)CANCL:取消键；2.2数字键盘操作区域(1)CLEAR:取消以前输入的数字；(2)ENTER:确认输入的数字；(3)+，-- :用于改变数字的正负或东西，南北；2.3菜单操作区域(1)雷达菜单操作(RADAR)；(2)电子海图菜单操作(ECDIS)，参看第二章；(3)NCC菜单操作，参看第三章；(4)导航菜单操作，参看第四章.2.4 MASTER主控导航操作区域(1)SPEED PILOT:速度控制键，（这个功能没有开发）(2)TRACK PILOT:航迹控制键，有三种航行模式可选择；(3)操纵杆:当按下SPEED PILOT和TRACK PILOT键后，MASTER灯亮，表示此雷达为导航雷达，可用操纵杆进行避让或转向.左右扳动改变航向，上下扳动改变旋回半径.2.5 TRACK PILOT:自动导航模式，只有当MASTER灯亮时，此功能有用.(1) HDG MODE:船艏向模式，船舶按照设定的艏向航行，而不考虑风流压角(和自动舵相似)；(2) CRS MODE:航向模式，船舶按照设定的航向航行，自动加风流压角，即船艏向是经过风流压修正后的；(3) TRACK MODE:自动导航模式，船舶按照预先设计的航线航行，根据各种航海传感器的数据自动修正风流压，在到达转向点前自动报警，尤其在到达转向点大约30秒前会有一个WHEEL OVER POINT警报显示，此时只要按TAKE OVER键，船舶自动转向下一个航向.如果船舶偏离了限制的角度或偏移量，它会报警并自动改成CRS MODE模式.3. 开机/关机3.1MULTIPILOT(9800型雷达)开机:开启电源开关(POWER)，在显示器上首先是图象测试，大约20秒；接着是设备的自身检测，并在显示器上显示以下内容：MULTIPILOT HARDWARE INITIALIZATIONMULTIPILOT OPERATING SYSTEM STARTUPECDIS STARTUP* 如有故障，即出现故障代码；大约10秒后显示和RADARPILOT(9600型雷达)一样的图象，并在底部显示CHART OFF；大约3分钟后按STDBY键MULTIPILOT就可以在RADAR模式操作。
组合导航
• 辅助工作站
海图修正 制定计划航线 选项：气象工作站可制定气象航线
服务器 记录对话和通信数据 • 航行数据记录器（VDR） 记录雷达数据 记录船舶运动
事故回放和分析
服务器
• 船舶最佳安全系统
气象工作站 最佳气象航线 最低油耗控制 船舶动态监测
GMDSS通信控制：通过SeaNET网连到主控制台 数字化海图桌 GPS/DGPS Loran C 导航传感器 Depth Sounder MK37 Gyrocompass Log
高精度组合导航系统 根据要求，取长补短， 一般组合导航系统 灵活转换 自动航行组合导航系统
***以自主式为基础
§4-3 组合导航计算机（P219）
技术基础：计算机 一、导航微机 已不仅是数据处理工具，而是组合导航的重要组 成部分
专用化 功能专用化：软件由硬件代替，处理速度 键盘 操作界面 通用化 易于操作 外设接口 多媒体
§4-6 自动航行组合导航实例
Litton Sperry Marine: VISION 2100 Integrated Bridge
Kelvin Hughes: Nucleus Integrated Navigation System
Ninas 9000 STN ATLAS Marine Electronics: Navigation System ATLAS NACOS Norcontrol: Norcontrol Bridgeline
§4 组合导航系统
组合导航系统：借助于现代（卡尔曼）滤波原理的计算机系统
产生：七十年代初提出组合导航思想 解决：大型油轮的安全性和经济性 功能：可实现自动定位、导航、避碰、驾驶、航线优选等。
基于DSP和ARM的车载组合导航计算机设计
Ｄｅ ｉ ｎ ｏ ｈｉｌ ｎ ｅ ｒ ｔ ｄ Ｎ ａ ｉ ａ ｉ ｎ Ｃｏ ｐ ｅ ｓ ｄ ｏ ｓｇ ｆＶｅ ｃｅ Ｉ ｔ ｇ ａ ｅ ｖ ｇ ｔｏ ｍ ｕｔ ｒ Ｂａ ｅ ｎ ＤＳＰ ｎ ａ ｄ ＡＲＭ
ＧＯＮＧ ａ— ｈ ｎ Ｂ ｉｃ ｕ ，Ｌ ｕ —ａ ， ＹＡＮ ｉｓ， ＭＡ ｎ ＩＳ ｉ ｏ ｌ Ｋａ—ｉ Ｒｏ ｇ
ｓ ｓｅ ｂ ｓ ｄ ｏ Ｐ ａ ｄ ＡＲＭ ｓ ｄ ｓ ｎ ｄ Ｔ ｅ ｓ ｓｅ ａ ｏ ｔｄ ＤＳ ｓ ｎ ｖ ｇ ｔ ｎ ｃ ｍｐ ｔｒ ｔ ｏ ｌｔ ｙ ｔｍ ａ ｅ ｎ ＤＳ ｎ ｗａ ｅ ｉ ｅ ． ｈ ｙ ｔｍ ｄ ｐ ｅ Ｐ ａ ａ ｉａ ｉ ｏ ｕ ｅ ｏ ｃ ｍｐ ｅｅ ｇ ｏ ｒｐ ｄ ｆ ｓ ｎ ａ ｄ ｃ ｌｕ ａ ｉｎ ｏ ａ ｉａ ｉｎ ｄ ｔ ． ａ ｉ ｕ ｉ ｎ ａ ｃ ｌ ｔ ｆｎ ｖ ｇ ｔ ａ ａ ＡＲＭ Ｐ ｏ ｌ ｔｄ ｔ ｅ ｓ ｓｅ ｌ ｖ ｌ ｏ ｔ ｌａ ｄ ｔ ｅ ａ ｑ ｉ — ｏ ｏ ｏ Ｃ Ｕ ｃ ｍｐ ｅｅ ｈ ｙ ｔｍ— ｅ ｎ ｒ ｎ ｈ ｃ ｕ ｓ ｅ ｃ ｏ ｉ ｔ ｎ ｏ Ｓ ａ ｄ ｉ ｇ ｎ ｏ ｍａ ｉｎ ｅｃ Ｃ Ｌ ｏ ｌ ｔｄ ｓ ｍｅ ｄ ｃ ｄ ｎ ， ｏ ｔ ｌａ ｄ ｔ ｅ ａ ｑ ｉ ｔ ｎ ｏ ｔ ｅ ｉ ｆＧＰ ｎ ｍａ ｅ ｉ ｆ ｒ ｔ ｔ ． Ｐ Ｄ ｃ ｍｐ ｅｅ ｏ ｅ ｏ ｉ ｇ ｃ ｎ ｒ ｎ ｈ ｃ ｕ ｓ ｉ ｆ ｏｈ ｒ ｏ ｏ ｏ ｉｏ
RTKPPP定位算法流程
1基础知识1.1GPS精密单点定位的基本原理GPS精密单点定位一般采用单台双频GPS接收机，利用IGS提供的精密星历和卫星钟差，基于载波相位观测值进行的高精度定位。
观测值中的电离层延迟误差通过双频信号组合消除，对流层延迟误差通过引入未知参数进行估计。
1.2时间系统RTKLIB内部使用GPST(GPST时间)用于GNSS的数据处理和定位算法。
数据在RTKLIB内部处理之前，需要转换成GPST时间。
使用GPST的原因是避免处理润秒。
RTKLIB使用以下结构体表示时间：typedef structtime_t time; /* time(s) expressed by standard time_t */double sec; /* fraction of second under 1 s */} gtime_t;GPST和UTC(Universal Time Coordinated)关系参考【图1】，参考【图2】：图1 转换关系公式图 2通过使用GPS导航信息中的UTC参数，GPST到UTC或者UTC到GPST之前的转换可以用更准确的表达方式，如【图3】。
图 3这些参数是由GPS导航消息提供的。
BDT（北斗导航卫星系统时间）BDT（北斗导航卫星系统时间）是一个连续的时间系统，没有润秒。
开始历元的时间是【UTC 2006年1月1号00:00:00】。
北斗时间计算公式【图4】：图 4UTC和GPST时间转换同上面的GPS一样，只不过UTC参数来自与北斗导航信息中。
坐标系统接收机和卫星的位置在RTKLIB中表示为在ECEF(地心地固坐标系)坐标系统中的X, Y, Z组件。
大地坐标到ECEF坐标的转换转换公式如【图5】。
第三个公式最后一行有错，应该为:(v(1 – e2)+h)sin图 5参数说明： a :地球参考椭球的长半径f : 地球参考椭球的扁平率h: 椭球高度:纬度: 经度当前版本的RTKLIB使用的值为【图6】：图 6图7 参考椭球体ECEF坐系到大地坐标的转换转换公式如【图8】图8本地坐标到ECEF坐标的转换在接收机位置的本地坐标，也被称为ENU坐标，通常使用在GNSS导航处理。
组合导航系统的计算程序代码
组合导航系统的计算程序代码function yy=ukf_IMUgps()%function ukf_IMUgps()% UKF在IMU/GPS组合导航系统中应用%% 以IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量；% 以GPS中的位置、速度为观测量。
%% 7,July 2008.clc% Initialise stateglobal we RN RM g fl deta wg Tt wt d ww v u W Rbl Ta wad=0; %验证循环次数%地球自转角速度we(rad/s):we=7.292115e-5;g=9.81; %地球重力加速度（m/s^2）a=6.378137e+6; %地球长半轴e2=0.0066944; %地球第一偏心率的平方%姿态角初始值（r,p,y）zitai=(pi/180).*[0 2.0282 196.9087];%姿态误差角fai=(pi/180).*[1/36 1/36 5/36]; %(100'',100'',500'')r=zitai(1)+fai(1);p=zitai(2)+fai(2);y=zitai(3)+fai(3);%当地坐标系（l）相对于载体坐标系（b）的转换矩阵：Rbl(在e,n,u坐标系下）Rbl=[cos(r)*cos(y)-sin(r)*sin(y)*sin(p) -sin(y)*cos(p) cos(y)*sin(r)+sin(y)*sin(p)*cos(r) cos(r)*sin(y)+sin(r)*cos(y)*sin(p) cos(y)*cos(p) sin(y)*sin(r)-cos(y)*sin(p)*cos(r) -cos(p)*sin(r) sin(p) cos(p)*cos(r)];%由转换矩阵Rbl计算四元数的初始值Q0=[q1,q2,q3,q4]'QQ=[0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3))0.25*(Rbl(3,2)-Rbl(2,3))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))0.25*(Rbl(1,3)-Rbl(3,1))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))0.25*(Rbl(2,1)-Rbl(1,2))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))];%IMU系统给出的初始值：速度（ve,vn,vu），位置（l,m,h）＝（经度，纬度,高度），姿态误差角fai(e,n,u)vIMU=[-21.775011 -71.631027 3.10094];point_IMU=[0.147022986 0.81837365 3122.826];T=1; %UKF滤波的采样周期（s）%陀螺常值漂移初始值tuo(e,n,u)(单位：。
校园导航系统的两个代码
#1 数据结构实验之校园导游咨询#include<stdlib.h>#include<stdio.h>#include<iostream.h>#include<string.h>#include<math.h>#include "stdio.h"#define null 0#define num 20#define maxdist 10000typedef struct{char data[num];int edges[num][num];}graph;void dijkstra(graph g,int n,int i,int d[num],int p[num]) {int s[num];int mindist,dist;int j,k,u;for(j=0;j<n;j++){d[j]=g.edges[i][j];s[j]=0;if((d[j]<maxdist)&&(d[j]!=0)) p[j]=i;elsep[j]=-1;}s[i]=1;for(j=0;j<n-1;j++){mindist=maxdist;u=i;for(k=0;k<n;k++)if((s[k]==0)&&(d[k]<mindist)) {u=k;mindist=d[k];}s[u]=1;for(k=0;k<n;k++)if(s[k]==0){dist=d[u]+g.edges[u][k];if(dist<d[k]){d[k]=dist;p[k]=u;}}}void opdijk(int v0,int n,int d[],int p[]) {int i,pre;for(i=0;i<n;i++)if(i!=v0){printf("\n%d",i);pre=p[i];while(pre!=-1){printf("<--%d",pre);/****************************/ pre=p[pre]; }if(d[i]==maxdist)printf("<--%d",v0);printf("\tshortestway:%d",d[i]);}}void floyd(graph g,int n,int d[][num],int p[][num]){int i,j,k;for(i=0;i<n;i++)for(j=0;j<n;j++){d[i][j]=g.edges[i][j];if((d[i][j]<maxdist)&&(d[i][j]!=0))p[i][j]=j;elsep[i][j]=-1;}for(i=0;i<n;i++)d[i][i]=0;for(k=0;k<n;k++)for(i=0;i<n;i++)for(j=0;j<n;j++)if(d[i][j]>d[i][k]+d[k][j]){d[i][j]=d[i][k]+d[k][j] ;p[i][j]=p[i][k];}}void opfloy(int n,int d[][num],int path[][num]) {int i,j,next;for(i=0;i<n;i++){printf("\n\nyuandian is v%d:",i);for(j=0;j<n;j++){printf("\nthe shortest way from%dto%dis:\n",i,j);printf("%d",i);next=path[i][j];while(next!=-1){printf("->%d",next);next=path[next][j];}if(d[i][j]==maxdist)printf("<-%d",j);/***************************/ printf("\t%d",d[i][j]);}}}void main(){graph g;int i,j,k,n;int d[num],p[num],sd[num][num],sp[num][num];//clrscr();n=11;g.data[0]='a';g.data[1]='b';g.data[2]='c';g.data[3]='d';g.data[4]='e';g.data[5]='f';g.data[6]='g';g.data[7]='h';g.data[8]='i';g.data[9]='j';g.data[10]='k';for(i=0;i<n;i++)for(j=0;j<n;j++)g.edges[i][j]=maxdist; for(j=0;j<n;j++)g.edges[i][j]=0;g.edges[0][2]=30;g.edges[0][5]=40;g.edges[0][1]=20;g.edges[1][5]=60;g.edges[1][4]=40;g.edges[1][6]=30;g.edges[2][3]=35;g.edges[2][7]=40;g.edges[3][10]=20;g.edges[3][8]=15;g.edges[4][9]=30;g.edges[4][8]=35;g.edges[4][10]=10;g.edges[5][10]=10;g.edges[5][8]=45;g.edges[5][2]=20;g.edges[6][0]=70;g.edges[7][0]=50;g.edges[8][9]=20;g.edges[9][1]=55;g.edges[10][0]=125;//clrscr();printf("\t --WELCOME TOU USE GUIDE!!--\n");printf("\t*****************************************\n");printf("\t* *\n");printf("\t*\tINFOMATION-i\tVISIT-v\t\t*\n\t*\t ABOUT ME-a\tQUIT-q\t\t*\n");printf("\t*\t\t\t\t\t*\n\t*\t\t\t\t\t*\n\t*\t\t\t\t\t*\n\t*\t\t\t\t\t*\n \t*\t\t\t\t\t*\n");printf("\t*****************************************\n");printf("\t*PLEASE ENTER AN ORDER:i? v? m? q?\t*\n");printf("\t*CLEAR THE SCREEN:c\t\t3\t*\n");printf("\t*****************************************\n");do{/*gets(t);*/switch(getchar()){case'a':{printf("NAME:****** CLASS:02computer(3)\n"); printf("PRIVATE EMAIL:devo800@\n"); }break;/* case'g':{FILE *tp;*//* break; */case'i':{FILE *tp;char ch;if((tp=fopen("d:\\info.txt","rt"))==null) {printf("\ncan't open!");getchar();exit(1);}ch=fgetc(tp);while(ch!=EOF){putchar(ch);ch=fgetc(tp);}fclose(tp);}break;case'c':break;case'v':{do{printf("\nINPUT JIE DIAN(0-10):\n"); scanf("%d",&i);{dijkstra(g,n,i,d,p);printf("\n\nyuandian is v%d:",i); opdijk(i,n,d,p);}}while(1);}break;default: exit(1);}}while(getchar());getchar();}*--------------------校园导游系统------------------*/#include<stdio.h>#include<process.h>#define INT_MAX 1000000#define n 10int cost[n][n];//边的值int shortest[n][n];//两点间的最短距离int path[n][n];//经过的景点的;void floyed();int display(int i,int j);void introduce()//introduce of the palce{ int a;printf("Place 1:凯旋门\n"); printf("Place 2:图书馆\n");printf("Place 3:行政楼\n"); printf("Place 4:飞翔门\n"); printf("Place 5:大广场\n");printf("Place 6:ABC教学楼\n"); printf("Place 7:体育场\n"); printf("Place 8:月亮湾\n"); printf("Place 9:English Coner\n"); printf("Place 10:树人广场\n");do{printf("您想查询哪个景点的详细信息?请输入景点编号:");scanf("%d",&a);getchar();switch(a){case 1:。
校园导航系统源代码
校园导航系统源代码#define INFINITY 10000#define MAX_VERTEX_NUM 40#define MAX 40#include<stdlib.h>#include<stdio.h>#include<conio.h>#include<string.h>typedef struct ArCell{int adj; /*路径长度 */}ArCell,AdjMatrix[MAX_VERTEX_NUM][MAX_VERTEX_NUM];typedef struct/*图中顶点表示主要景点,存放景点的编号、名称、简介等信息, */{char name[30];int num;char introduction[100];/*简介*/}infotype;typedef struct{infotype vexs[MAX_VERTEX_NUM];AdjMatrix arcs;int vexnum,arcnum;}MGraph;MGraph b;void cmd(void);MGraph InitGraph(void);void show1();void list();void Menu(void);void ShortestPath_DIJ(MGraph * G);void Search(MGraph *G);int LocateVex(MGraph *G,char* v);/**********主函数************************/void main(void){system("color 5f"); /*修改控制台的颜色信息,改为白字蓝底的模式*/system("mode con: cols=140 lines=130"); /*设置批处理运行时窗口大小的*/cmd();}/********自定义函数***************//* cmd函数(根据目录选择要进行的项目)*/void cmd(void){char k;b=InitGraph();Menu();while(1){scanf("\n%c",&k);switch(k){case'x':system("cls");show1();Menu();list();ShortestPath_DIJ(&b);printf("---------------------------------欢迎您的使用--------------------------------\n");printf("\n请您继续选择服务:");break;case'y':system("cls");Menu();list();Search(&b);printf("---------------------------------欢迎您的使用--------------------------------\n");printf("\n请您继续选择服务:");break;system("cls");printf(" ┏━━━━━━━━━━━━━━━━━━━━┓\n");printf(" ┃感谢使用┃\n");printf(" ┃安徽建筑术大学┃\n");printf(" ┃智能导航系统┃\n");printf(" ┗━━━━━━━━━━━━━━━━━━━━┛\n");exit(0);default:printf("输入信息错误！\n请输入x或y或z.\n");break;}}}/* 迪杰斯特拉算法来计算出起点到各个顶点之间的最短路径,v0为起点 */void ShortestPath_DIJ(MGraph * G){int v,w,i,min,t=0,x,flag=1,v0,v1,have[100],k;int final[20], D[20], p[23][23];while(flag)printf("请输入起始景点编号:\n");scanf("%d",&v0);if(v0<0||v0>G->vexnum)printf("景点编号不存在!");printf("请输入终止景点编号:\n");scanf("%d",&v1);if(v1<0||v1>G->vexnum)printf("景点编号不存在!");if(v0>=0&&v0<G->vexnum&&v1>=0&&v1<G->vexnum) flag=0;}for(v=0;v<G->vexnum;++v){final[v]=0;D[v]=G->arcs[v0][v].adj;for(w=0;w<G->vexnum;++w)p[v][w]=INFINITY;if(D[v]<INFINITY){p[v][v0]=1;p[v][v]=1;}D[v0]=0;final[v0]=1;have[0]=v0;for(i=1;i<G->vexnum;++i){min=INFINITY;for(w=0;w<G->vexnum;++w)if(!final[w])if(D[w]<min){v=w;min=D[w];}final[v]=1;have[k]=v;k++;for(w=0;w<G->vexnum;++w)if(!final[w]&&((min+(G->arcs[v][w].adj))<D[w])){D[w]=min+G->arcs[v][w].adj;for(x=0;x<G->vexnum;x++)p[w][x]=p[v][x];p[w][w]=1;}for(i=0;i<G->vexnum;i++){if(p[v1][have[i]]==1){printf("-->%s",G->vexs[have[i]].name);} }if((v1-v0)==1)printf("\n路径长度:%d\n",G->arcs[v0][v1]);else printf("\n路径长度:%d\n",D[v1]);}//ShortestPath_DIJ end/*查找函数的建立 */void Search(MGraph *G){int k,flag=1;while(flag){printf("请输入要查询的景点编号:");scanf("%d",&k);if(k<0||k>G->vexnum){printf("景点编号不存在!请重新输入景点编号:");scanf("%d",&k);}if(k>=0&&k<G->vexnum)flag=0;}printf("┏━━┳━━━━━━━━┳━━━━━━━━━━━━━━━━━━━━━━━━━━━━━┓\n");printf("┃编号┃景点名称┃简介┃\n");printf("┃%-4d┃%-16s┃%-58s┃\n",G->vexs[k].num,G->vexs[k].name,G->vexs[k].introduction);printf("┗━━┻━━━━━━━━┻━━━━━━━━━━━━━━━━━━━━━━━━━━━━━┛\n");}//Search endvoid show1(){ printf("\t\t ★★欢迎使用安徽建筑大学智能导航系统★★\n");printf("\t\t\t安徽建筑大学南校区简略版平面图\n\n");printf("\t 学校北门\n");printf("\t ┃ \n");printf("\t ┏━━北食堂━━━━━━━━━宿舍楼1\n");printf("\t ┃┃\n");printf("\t ┃宿舍楼2\n");printf("\t ┃━━━━━━九月桥━━━━━┃\n");printf("\t ┃┃\n");printf("\t 主教楼┃ \n");printf("\t ┃┃\n");printf("\t 日月广场┃\n");printf("\t ┃┃\n");printf("\t 图书馆┃\n");printf("\t ┃┃\n");printf("\t 还在建┃\n");printf("\t ┃┃\n");printf("\t 南食堂━━━━━━━━━━━━宿舍楼15\n");printf("\t ┃┃\n");printf("\t ┃┃\n");printf("\t ━━━━━━━━━━━━━━南苑超市\n");}void list(){printf("学校景点列表:\n");printf("0:学校北门");printf("1:宿舍楼1 ");printf("2:宿舍楼2 ");printf("3:宿舍楼15 ");printf("4:九月桥\n");printf("5:主教楼 ");printf("6:图书馆 ");printf("7:日月广场 ");printf("8:体育场\n");printf("9:还在建");printf("10:南食堂 ");}/*目录函数的构建*/void Menu(){printf("\n 安徽建筑大学南校区导游图\n");printf(" ┏━━━━━━━━━━━━━━━━━━━━┓\n");printf(" ┃ x.选择出发点和目的地┃\n");printf(" ┃ y.查看景点信息┃\n");printf(" ┃ z.退出系统┃\n");printf(" ┗━━━━━━━━━━━━━━━━━━━━┛\n");printf("请选择服务");}/*MGraph函数（图的构建）*/MGraph InitGraph(void){MGraph G;int i,j;G.vexnum=17; //顶点是17个G.arcnum=25; //弧线有25个for(i=0;i<G.vexnum;i++)G.vexs[i].num=i;strcpy(G.vexs[0].name,"学校北门");strcpy(G.vexs[0].introduction,"学校的正门，气势宏伟");strcpy(G.vexs[1].name,"宿舍楼1");strcpy(G.vexs[1].introduction,"睡觉的地方");strcpy(G.vexs[2].name,"宿舍楼2");strcpy(G.vexs[2].introduction,"睡觉的地方");strcpy(G.vexs[3].name,"宿舍楼15");strcpy(G.vexs[3].introduction,"，，，，，，，，");strcpy(G.vexs[4].name,"九月桥");strcpy(G.vexs[4].introduction,"连接生活区和教学区的桥。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
组合导航系统的计算程序代码function yy=ukf_IMUgps()%function ukf_IMUgps()% UKF在IMU/GPS组合导航系统中应用%% 以IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量；% 以GPS中的位置、速度为观测量。
%% 7,July 2008.clc% Initialise stateglobal we RN RM g fl deta wg Tt wt d ww v u W Rbl Ta wad=0; %验证循环次数%地球自转角速度we(rad/s):we=7.292115e-5;g=9.81; %地球重力加速度（m/s^2）a=6.378137e+6; %地球长半轴e2=0.; %地球第一偏心率的平方%姿态角初始值（r,p,y）zitai=(pi/180).*[0 2.0282 196.9087];%姿态误差角fai=(pi/180).*[1/36 1/36 5/36]; %(100'',100'',500'')r=zitai(1)+fai(1);p=zitai(2)+fai(2);y=zitai(3)+fai(3);%当地坐标系（l）相对于载体坐标系（b）的转换矩阵：Rbl(在e,n,u坐标系下）Rbl=[cos(r)*cos(y)-sin(r)*sin(y)*sin(p) -sin(y)*cos(p) cos(y)*sin(r)+sin(y)*sin(p)*cos(r) cos(r)*sin(y)+sin(r)*cos(y)*sin(p) cos(y)*cos(p) sin(y)*sin(r)-cos(y)*sin(p)*cos(r) -cos(p)*sin(r) sin(p) cos(p)*cos(r)];%由转换矩阵Rbl计算四元数的初始值Q0=[q1,q2,q3,q4]'=[0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3))0.25*(Rbl(3,2)-Rbl(2,3))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))0.25*(Rbl(1,3)-Rbl(3,1))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))0.25*(Rbl(2,1)-Rbl(1,2))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))];%IMU系统给出的初始值：速度（ve,vn,vu），位置（l,m,h）＝（经度，纬度,高度），姿态误差角fai(e,n,u)vIMU=[-21.775011 -71.631027 3.10094];point_IMU=[0. 0. 3122.826];T=1; %UKF滤波的采样周期（s）%陀螺常值漂移初始值tuo(e,n,u)(单位：。
/s）tuo=[0 0 0];%陀螺一阶相关时间Tt(s)Tt=[60 60 60];%加速度计常值漂移初始值jiasu(e,n,u)(单位：m/s^2）jiasu=[0 0 0];%加速度计一阶反相关时间Ta(s)Ta=[60 60 60];%IMU系统的状态向量Xx=[vIMU point_IMU fai tuo jiasu]';%观测量噪声V的斜方差矩阵R=[];%GPS系统的量测值Z(vn,ve,vd,m,l,h)[z Rz]=gps_tiqushuju;%Vk的噪声序列方差为：RkRz=(Rz./T);%陀螺常值漂移wt(e,n,u)wt=(pi/180).*[1/(3600) 1/(3600) 1/(3600)]; % 1 (。
/h)%陀螺常值漂移误差wtt(e,n,u)wtt=(pi/180).*[0.08/(3600) 0.08/(3600) 0.08/(3600)]; % 0.08 (。
/h)%加速度计常值漂移wa(e,n,u)wa=[2e-6 2e-6 2e-6]; % 200μg (2e-6 m/s^2)%加速度计常值漂移误差waa(e,n,u)waa=[2e-6 2e-6 2e-6]./4; % 50μg (0.5e-6 m/s^2)%姿态误差角噪声wgwg=wt;%状态向量X的斜方差矩阵P = eye(length(x))*eps; % note: for stability, P should never be quite zero%速度误差：（0.1m/s) 位置误差：水平（1m),高度（5m)u=[0.1 0.1 0.1 0. 0. 2 wg wtt waa]';%IMU系统在载体坐标系下的比力值输出值fbfb=[];%IMU系统中陀螺输出量wib=[]; %为载体坐标系（b）相对于惯性坐标系（i）的角速度向量[f w]=IMU_tiqushuju; %IMU系统输出的比力值和角速度%%%%%%%%%通过GPS观测值计算得到的姿态角zitaijiao_gps=xlsread('D:\myfile\UKF\kalmanfilter_MATLAB\germany_ukf\原始数据\');%%%------------------------------------------------------------%% 循环开始：for 1:noutx=[];outp=[];detajiao=zeros(3,1);NN =1000;for i=1:NNoutx=[outx;x'];outp=[outp;diag(P)'];%大地子午圈曲率半径：RMRM=a*(1-e2)/(1-e2*(sin(x(5)))^2)^(2/3);%地球卯酉圈的曲率半径：RNRN=a/sqrt(1-e2*(sin(x(5)))^2);%当地坐标系下的比力值fl%IMU系统在载体坐标系下的比力值输出值fb转换到当地坐标系（l）下flfb=f(i,:)';%fl=Rbl*(fb+[x(13) x(14) x(15)]'); %初始对准，比力值加上加速度计的测量偏差fl=Rbl*fb;%计算IMU的速度、位置输出减去GPS相应的输出值：deta(ve,vn,l,h)zd=z(i,:)';deta=x([1 2 5 6],:)-zd([1 2 5 6],:);%观测值zz,及观测噪声Rzz=z(i+1,:)';v=Rz(i+1,:)';%zitai_v=[0.001 0.0266 0.9664]'; %GPS观测值姿态角的误差zitai_v=[0.00001 7.0983e-004 0.0006]'; %GPS观测值姿态角的误差v=[v;zitai_v];R=diag(v.^2);%%通过GPS观测值，计算得到roll=0 ，pitch=atan(ve/vn)，yaw=asin(h12/s12) ，将姿态误差角加入观测量中进行滤波计算zz=[zz;detajiao];%%%%GPS计算得到的姿态角z_zitai=zitaijiao_gps(i+1,:);%%IMU系统的力学编排方程和姿态误差方程离散化之后的截断误差：ve=x(1);vn=x(2);vu=x(3);l=x(4);m=x(5);h=x(6);faie=x(7);fain=x(8);faiu=x(9);tuo1=x(10);tuo2=x (11);tuo3=x(12);fl1=fl(1);fl2=fl(2);fl3=fl(3);deta1=deta(1); deta2=deta(2); deta3=deta(3); deta4=deta(4);wg1=wg(1);wg2=wg(2);wg3=wg(3);jiasu1=x(13);jiasu2=x(14);jiasu3=x(15);Ta1=Ta(1);Ta2=Ta(2);Ta3=Ta(3);wa1=wa(1);wt2=wa(2) ;wt3=wa(3);Q=diag((u).^2);% predict[x,P] = predict(x, P,u, Q, T);% update[x,P] = update(x, P, zz, R);%%xx=x;x=xx(1:15,1);u=xx(16:30,1);%u(1)=u(1)+x(13);%u(2)=u(2)+x(14);%u(3)=u(3)+x(15);%u(7)=x(10);%u(8)=x(11);%u(9)=x(12);PP=P;P=PP(1:15,1:15);%利用四元数Q计算转换矩阵Rbl%首先计算在当地坐标系（l）下的角速度向量wbl%地固坐标系（e）相对于当地坐标系（l）的转换矩阵：Rel=Rle'%Rel=[-sin(x(4)) cos(x(4)) 0% -sin(x(5))*cos(x(4)) -sin(x(5))*sin(x(4)) cos(x(5))% cos(x(5))*cos(x(4)) cos(x(5))*sin(x(4)) sin(x(5))];wie=[0 0 we]'; %wie 为地球自转角速度向量%%%%%%%omiga12=[];for k=0:1wib=w(i+k,:)'+T.*[x(10) x(11) x(12)]'; %初始对准，角速度加上陀螺漂移wiel=[0 we*cos(x(5)) we*sin(x(5))]';%wiel=Rel*wie;well=[-x(2)/(RM+x(6)) x(1)/(RN+x(6)) x(1)*tan(x(5))/(RN+x(6))]';will=wiel+well;wlb=wib-Rbl'*will;%四元数的更新%计算反对称矩阵omigaomiga=[0 wlb(3) -wlb(2) wlb(1)-wlb(3) 0 wlb(1) wlb(2)wlb(2) -wlb(1) 0 wlb(3)wlb(1) wlb(2) wlb(3) 0];omiga12=[omiga12,omiga];endomiga1=omiga12(1:4,1:4);omiga2=omiga12(1:4,5:8);%采用四阶龙格－库塔法数值积分解Q(K+1)=+(0.5*T.*(omiga1+omiga2)+8^(-1)*T^2.*(omiga1^2+omiga1*omiga2+omiga2^2)+48^(-1)*T^3 .*(omiga1^2*omiga2+omiga1*omiga2^2)+384^(-1)*T^4.*(omiga1^2*omiga2^2))*;%由Q(k+1)可得Rbl(k+1)Rbl=[(1)^2+(2)^2-(3)^2-(4)^2 2*((2)*(3)-(1)*(4)) 2*((2)*(4)+(1)*(3))2*((2)*(3)+(1)*(4)) (1)^2-(2)^2+(3)^2-(4)^2 2*((3)*(4)-(1)*(2))2*((2)*(4)-(1)*(3)) 2*((2)*(1)+(4)*(3)) (1)^2-(2)^2-(3)^2+(4)^2];%由Rbl(k＋1）可得姿态角，（翻滚角r，俯仰角p，航向角y）k+1%实际导航系（p系）相对于理想导航系（n系）存在误差角fai(e,n,u),Cpn为校正矩阵%当方位角为大失准角时Cpn=[cos(x(9)) -sin(x(9)) x(8)*cos(x(9))+x(7)*sin(x(9))sin(x(9)) cos(x(9)) x(8)*sin(x(9))-x(7)*cos(x(9))-x(8) x(7) 1];%%当三个方向的失准角为小角度时Cpnn=[1 -x(9) x(8)x(9) 1 -x(7)-x(8) x(7) 1];if abs(x(9))*180*60/pi < 10 %当姿态误差角（u），即方位角失准角小于10’时的情况；Rbl=Cpnn*Rbl; %小失准角elseRbl=Cpn*Rbl; %大失准角endzitai0=[atan(-Rbl(3,1)/Rbl(3,3))asin(Rbl(3,2))atan(-Rbl(1,2)/Rbl(2,2))];if Rbl(2,2)<0 & zitai0(3)<0zitai0(3)=zitai0(3)+pi;endif Rbl(2,2)<0 & zitai0(3)>0zitai0(3)=zitai0(3)-pi;end% detajiao=zitai0-z_zitai';% detajiao(1)=0;% detajiao=Rbl*detajiao;% if abs(detajiao(2)) < abs(x(8))% x(8)=detajiao(2);% %zitai0(2)=z_zitai(2);% end%if abs(detajiao(3)) < abs(x(9))% x(9)=detajiao(3);%zitai0(3)=z_zitai(3);%endzitai=[zitai;zitai0'];end%将协方差转换成标准差outp=(outp).^(1/2);%将位置误差中的经度和纬度形式转换成当地坐标系（e,n,u)形式for k=1:NN%大地子午圈曲率半径：RMRM=a*(1-e2)/(1-e2*(sin(outx(k,5)))^2)^(2/3);%地球卯酉圈的曲率半径：RNRN=a/sqrt(1-e2*(sin(outx(k,5)))^2);outp(k,4)=(RN+outx(k,6))*cos(outx(k,5))*outp(k,4);outp(k,5)=(RM+outx(k,6))*outp(k,5);end%绘制高度图figureplot(outx(:,6),'r-.');title('UKF计算的高度(germany-data)');figureplot((180/pi)*outx(:,8),'r-');title('UKF姿态误差角pitch（度）(germany-data)'); % 绘制图：水平运动轨迹figureplot(outx(:,4),outx(:,5),'b-');title('UKF Level of Movement(Germany-data)'); xlabel('Longitude（rad）');ylabel('Latitude（rad）');%hold on%gpsweizhi=xlsread('原始数据\');%plot(gpsweizhi(:,4),gpsweizhi(:,5),'r-');%hold off%绘制图；UKF速度误差figuresubplot(3,1,1)plot(outp(:,1),'b-');title('UKF Velocity Error(Germany-data)');ylabel('Ve(m/s)');subplot(3,1,2)plot(outp(:,2),'b-');ylabel('Vn(m/s)');subplot(3,1,3)plot(outp(:,3),'b-');xlabel('t(s）');ylabel('Vu(m/s)');%绘制图；UKF位置误差figuresubplot(3,1,1)plot(100.*outp(:,4),'b-');title('UKF Location Error(Germany-data)'); ylabel('x(cm)');subplot(3,1,2)plot(100.*outp(:,5),'b-');ylabel('y(cm)');subplot(3,1,3)plot(100.*outp(:,6),'b-');xlabel('t(s）');ylabel('z(cm)');%绘制图；UKF姿态角误差figuresubplot(3,1,1)plot(3600*(180/pi)*outp(:,7),'b-');title('UKF Attitude Error(Germany-data)'); ylabel('roll(second)');subplot(3,1,2)plot(3600*(180/pi)*outp(:,8),'b-');ylabel('pitch(second)');subplot(3,1,3)plot(3600*(180/pi)*outp(:,9),'b-');xlabel('t(s）');ylabel('yaw(second)');xlswrite('计算结果\output_x.xls',outx); xlswrite('计算结果\output_p.xls',outp); xlswrite('计算结果\output_',zitai);%%function [x,P] = predict(x, P,u, Q, T)%进行无迹变换P = blkdiag(P,Q);x=[x;u];% Perform unscented transform[x,P] = unscented_transform(@predict_model, [], x, P, T); % notice how the additional parameter 'T' is passed to the model%%%function [y, Y] = unscented_transform(func, dfunc, x,P,varargin):注意其中'dfunc'不知道？P(1:15,1:15)=P(1:15,1:15)+Q;%%function [x,P] = update(x, P, z, R)[x,P] = unscented_update(@observe_model,@observe_residual, x, P, z, R);%function x = predict_model(x, T)global wt Tt Ta wa Rbl%计算预报值% 由于UKF使用的是离散时间非线性模型，% 因此需要对IMU/GPS组合系统模型进行离散化处理；% 这里采用四阶Runge-kuta法以数值积分的形式实现。