伪距定位算法(matlab版)
在各种伪距定位算法中,最小二乘法是一种比较简单而广泛的方法,该算法可以分为以下几步:
1、准备数据与设置初始值
这里准备数据,主要是对于各颗可见卫星,收集到它们在同一时刻的伪距测量值,计算测量值的各项偏差、误差成分的校正量,然后计算出误差校正后的伪距测量值,这里假设伪距为理想距离加上随机高斯误差。设置初始值,假设大概知道位置坐标,则设定其为初始值,也可根据上一次定位结果设定;若什么都不了解,那么初值设置为0,只不过多几次迭代过程罢了。
2、非线性方程组线性化(不详细解释,就是得到雅克比矩阵)。
3、求解线性方程组,此处运用最小二乘法。
4、更新非线性方程组的根
5、判断牛顿迭代的收敛性,用定位结果的二范数是否小于定位精度为判定标准。
贴出伪距定位算法的MATLAB代码:
1、主函数dingwei_main.m
1 %伪距定位算法(多星) 2 %2014.7.23 3 %距离单位为km 4 %----------------------------begin----------------------------------------- 5 clear;clc; 6 c=299792.458; %光速 7 8 %通过星历参数解算到所在地可见卫星的坐标位置 9 sat13=[-7134.529244 16113.648836 23709.205570]; 10 sat22=[-22383.700040 18533.233168 5307.245613]; 11 sat23=[-5384.901317 28971.622323 2079.796362]; 12 sat14=[637.466571 28016.053841 9347.297933]; 13 sat12=[-11568.199533 -3328.511543 26977.312423]; 14 sat21=[-28908.916747 -577.061760 6051.375658]; 15 sat5=[-1205.651181 28296.890128 -8397.025036]; 16 sat4=[16456.527324 12347.282494 21199.173063]; 17 18 sat=[sat13;sat22;sat23; sat14; sat12; sat21; sat5; sat4];%多卫星位置矩阵 19 %所在地实际大地坐标,用来与定位结果作比较 20 nanjing=[-2604.298533 4743.297217 3364.978513]; 21 %理想伪距测量值 22 r0 = zeros(8,1); 23 for i = 1:8 24 r0(i,1)=norm(sat(i,:)-nanjing); 25 end 26 %加入零均值,方差为80的随机高斯分布,模拟含有误差的伪距r 27 r = r0 + sqrt(80)*randn(size(r0))*1e-3; 28 %-------------------------------------------------------------------------- 29 %Newton迭代法 30 %设定迭代初值,若无法估计则全部假设为0 31 xyzt=[0 0 0 0]; 32 33 for i = 1:100 % 最大迭代次数设为100次 34 f = dingwei_fun(xyzt,sat,r); 35 df = dingwei_dfun(xyzt,sat); 36 37 %左除,具有良好的数值稳定性,在MATLAB中此已经为最小二乘意义下的解 38 %delta(xyzt)=G^(-1)*b 39 delta=-df\f; 40 xyzt(1)=xyzt(1)+delta(1); 41 xyzt(2)=xyzt(2)+delta(2); 42 xyzt(3)=xyzt(3)+delta(3); 43 xyzt(4)=xyzt(4)+delta(4); 44 45 p=norm(delta); %定位精度 46 if (p<1e-10) 47 break; 48 end 49 end 50 51 disp('迭代次数为') 52 i 53 disp('用户位置为') 54 xyzt(1:3) 55 disp('用户钟差为') 56 xyzt(4) 57 %--------------------------------end---------------------------------------
2、非线性方程组dingwei_fun.m
1 function f=dingwei_fun(xyzt,sat,r) 2 %多卫星定位方程函数 3 %xyzt(1:3)为用户位置(km) 4 %xyzt(4)为用户钟差(s) 5 %r为测量得到的伪距(km) 6 %sat为卫星数据 7 c=299792.458; %光速 8 9 [m,n]=size(sat); 10 for i=1:m 11 f(i)=norm(sat(i,:)-xyzt(1:3))+c*xyzt(4)-r(i); 12 end 13 f=f';
3、方程组偏导数矩阵
1 %多颗卫星定位的偏导数矩阵 2 function df=dingwei_dfun(xyzt,sat) 3 %xyzt为用户位置及钟差 4 %sat为卫星数据 5 c=299792.458; %光速 6 7 [m,n]=size(sat); 8 xyz=xyzt(1:3); 9 for i=1:m 10 for j=1:3 11 df(i,j)=(xyz(j)-sat(i,j))/norm(sat(i,:)-xyz(j)); 12 end 13 end 14 df(:,4)=c; %线性函数ct,对t求偏导为c
转载于:https://www.cnblogs.com/iamlsj/p/3870023.html
伪距定位算法(matlab版)相关推荐
- 卫星伪距定位matlab,GPS卫星运动及定位matlab仿真.doc
书山有路勤为径! PAGE GPS卫星运动及定位matlab仿真 摘要 全球定位系统是具有全球性.全能性.全天候优势的导航定位.定时和测速系统,现在在全球很多领域获得了应用. GPS卫星的定位是一个比 ...
- aoa定位算法matlab仿真,基于信号到达角度(AOA)的定位算法研究
内容摘要:基于信号到达角度(AOA)的定位算法是一种常见的无线传感器网络节点自定位算法,算法通信开销低,定位精度较高.由于各种原因,估测的多个节点位置可能存在不可靠位置,提出了一种改进的基于信号到达角 ...
- 【定位问题】基于RSSI均值的等边三角形定位算法matlab系统
1 简介 为了提高无线传感网络的定位精度从提高测量精度.改善信标N点分布的角度提出了一种基于RSSI均值的等边三角形定位算法.该算法引入信号强指( RSSI)敏感区和非敏感区概念采用高斯模型对非敏感区 ...
- tdoa/aoa定位的扩展卡尔曼滤波定位算法matlab源码,TDOA/AOA定位的扩展卡尔曼滤波定位跟踪算法Matlab源码...
TDOA/AOA定位是无线定位领域里使用得最多的一种定位体制,扩展卡尔曼滤波器是最经典的非线性滤波算法,可用于目标的定位和动态轨迹跟踪,GreenSim团队实现了该算法,本源码由GreenSim团队原 ...
- 人工势场算法 Matlab版源码
人工势场算法,用于路径规划 main.m程序 %初始化车的参数 Xo=[0 0];%起点位置 k=15;%计算引力需要的增益系数 K=0;%初始化 m=5;%计算斥力的增益系数,都是自己设定的. Po ...
- 基于sift的医学图像配准算法matlab版
当进行医学图像配准时,SIFT (Scale-Invariant Feature Transform) 算法是一种广泛使用的特征提取和匹配算法.它具有良好的尺度不变性和鲁棒性,可以在不同的图像尺度下检 ...
- 超宽带定位算法matlab,毕业设计_--超宽带(UWB)无线定位技术.doc
毕 业 设 计 题目: 超宽带(UWB)无线定位技术 学 校 院 系 专 业 姓 名 学 号 2012 年 6 月 论文题目: 超宽带(UWB)无线通信技术 专 业: 学 生: 签名: 指导教师: 签 ...
- 北斗导航 | 北斗伪距定位、测速与授时——PNT:最小二乘法、加权最小二乘:伪距单点定位
================================================ 博主github:https://github.com/MichaelBeechan 博主CSDN:h ...
- matlab版开源GNSS_SDR-masterGPS软件接收机代码解读
相关代码解读好像并不多,这是另一个博主的,内容比较详细.https://blog.csdn.net/weixin_43179259/article/details/116794856 本文仅介绍依据谢 ...
最新文章
- C语言基础(12)-输入和输出
- 动态代理以及对应Spring中AOP源码分析
- 组合模式(Composition)
- Linux下源码编译安装新版libxcb
- UVALive 7324	ASCII Addition (模拟)
- CentOS 6.4用源代码安装LNMP环境
- moco常用配置参数及实战
- 判断两根线段是否相交
- java ffmpeg amr mp3_使用ffmpeg实现 amr转mp3
- win10系统自动打开代理服务器的解决方法
- JavaScript 对象
- 工业以太网串口网关ENB-301MT
- 放开二妻你会拼命一点儿工作么|独秀日记
- 清越科技将开启申购:预计募资约8亿元,高裕弟为实际控制人
- 【题解】[POI2006」Tet-Tetris 3D
- win7 自建wifi
- PHY驱动调试之 --- PHY控制器驱动(二)
- 那些著名或非著名的iOS面试题(中)
- 中兴智能视觉大数据研发人脸识别门禁考勤机、精准的人脸识别对比
- DeepDive教程3--模块与细节
热门文章
- 构造函数初始化结构体
- C++中list的使用方法及常用list操作总结
- java 实体属性个数_?Java中比较实用实体转换工具介绍
- android自动创建桌面,Android应用启动后自动创建桌面快捷方式的实现方法
- php的内存划分,解析PHP中的内存管理,PHP动态分配和释放内存
- 024_JDK的equals方法
- java 泛型 get()_Java泛型,get类的泛型参数
- 关于程序为什么要代码段,程序段
- 宁波大红鹰学院计算机毕设,网络文化对价值观的影响调查报告马克思主义基本原理概论》课程调查实践报告大学毕业设计.doc...
- redhat linux创建磁盘分区,redhat6.4 挂载硬盘,创建新分区,删除分区