伪距定位算法(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
