伪距定位算法(matlab版) |
您所在的位置：网站首页 › matlab卫星空间位置计算 › 伪距定位算法(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 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |