利用泰勒展开和牛顿迭代求解定位方程matlab仿真

1.实验名称

利用泰勒展开和牛顿迭代求解定位方程

2.实验背景

已知GPS接收机观测到的伪距方程，且伪距定位方程组是非线性方程组，同时方程数大于未知量数（即为超定方程），利用泰勒展开和牛顿迭代对定位方程进行求解，并对仿真结果进行分析。

3.实验原理

3.1牛顿迭代法求解非线性方程组

3.2伪距

伪距是GPS接收机对卫星信号的一个最基本的距离测量值。伪距是由于卫星时钟与接收机时钟不同步产生的，测得的距离含有时钟误差和大气层折射延迟，而非“真实距离”，故称为伪距。

3.3 伪距定位原理

定义传播时延、星钟误差校正后的伪距观测方程为：

3.4 伪距定位算法

在得到N个卫星的观测量后，即可得到N个伪距测量方程。采用最小二乘法，N个非线性方程组的解算可以分为 5 步：
(1)数据准备与设计初值：
测量伪距，为接收机当前位置及接收机钟差设置初值。初值的选择：设置上次定位结果、用户输入位置和时间、卫星坐标均值在地面上的投影，或者全部设置为 0。
(2)非线性方程组线性化：
在当前历元k次牛顿迭代中，将方程进行线性化(考虑第n个方程)：


全部观测方程也可以表示成矩阵方程组的形式。
(3)求解线性方程组：
利用最小二乘法求解GPS伪距定位线性矩阵方程组，若各个卫星测量值的误差方差以及权重已被确定，则可采用加权最小二乘算法求解。
(4)更新非线性方程组的根：
对接收机位置坐标及钟差进行更新。

(5)判断牛顿迭代的收敛性：

4.matlab仿真

4.1 仿真过程

（1）设GPS 接收机观测到的伪距方程为：

伪距定位方程组含两个未知数，三个方程，方程数大于未知量数，通过牛顿迭代与最小二乘法求解。

sat1=[4 2];
sat2=[3 5];
sat3=[-3 2];
sat=[sat1;sat2;sat3];%多卫星位置矩阵

（2）牛顿迭代法，设定迭代初值全部为0，即x=0,y=0,z=0;设最大迭代次数为100次;设迭代停止条件为误差小p<1e-10。
1）定位方程函数：

function f=position_f(xyt,sat,r)
	c=299792.458; %光速
	[m]=size(sat);
	for i=1:m
		f(i)=norm(sat(i,:)-xyt(1:2))+c*xyt(3)-r(i);
	end
	f=f';

其中，xyt(1:2)为用户位置(km)，xyt(3)为用户钟差(s)，r 为测量得到的伪距(km)，sat 为卫星数据
2）多颗卫星定位的偏导数矩阵：

function df=position_df(xyt,sat)
	c=299792.458; %光速,km/s
	[m]=size(sat);
	xy=xyt(1:2);
	for i=1:m
		for j=1:2
			df(i,j)=(xy(j)-sat(i,j))/norm(sat(i,:)-xy(j));
		end
	end
	df(:,3)=c; %线性函数 ct，对 t 求偏导为 c

3）迭代过程：

delta=-df\f;
xyt(1)=xyt(1)+delta(1);
xyt(2)=xyt(2)+delta(2);
xyt(3)=xyt(3)+delta(3);
p=norm(delta); %定位精度
if (p<1e-10)
break;
 end
end

4.2 仿真结果

最终迭代次数为23次，小于100次，故仿真结果正确。
用户位置即伪距定位方程组的根为x=1,y=-2。

此时误差为7.7575e-11，小于1e-10，故仿真结果正确。

4.3 仿真结论

由于此伪距方程组比较简单，若直接把初值设为x=1,y=-2时，迭代次数为1且误差为 0，故x=1,y=-2满足条件。初值的选择很重要，可根据上次定位结果、用户输入位置和时间、卫星坐标均值在地面上的投影等确定，初值选取得越好，迭代次数越少，越快在允许精度误差范围内收敛于真值。

matlab代码链接: https://blog.csdn.net/qq_44394952/article/details/122259721?spm=1001.2014.3001.5502.