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

1.实验名称

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

2.实验背景

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

3.实验原理

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

利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第1张图片
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第2张图片
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第3张图片

3.2伪距

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

3.3 伪距定位原理

定义传播时延、星钟误差校正后的伪距观测方程为:
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第4张图片

3.4 伪距定位算法

在得到N个卫星的观测量后,即可得到N个伪距测量方程。采用最小二乘法,N个非线性方程组的解算可以分为 5 步:
(1)数据准备与设计初值:
测量伪距,为接收机当前位置及接收机钟差设置初值。初值的选择:设置上次定位结果、用户输入位置和时间、卫星坐标均值在地面上的投影,或者全部设置为 0。
(2)非线性方程组线性化:
在当前历元k次牛顿迭代中,将方程进行线性化(考虑第n个方程):
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第5张图片
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第6张图片
全部观测方程也可以表示成矩阵方程组的形式。
(3)求解线性方程组:
利用最小二乘法求解GPS伪距定位线性矩阵方程组,若各个卫星测量值的误差方差以及权重已被确定,则可采用加权最小二乘算法求解。
(4)更新非线性方程组的根:
对接收机位置坐标及钟差进行更新。
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第7张图片
(5)判断牛顿迭代的收敛性:
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第8张图片

4.matlab仿真

4.1 仿真过程

(1)设GPS 接收机观测到的伪距方程为:
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第9张图片
伪距定位方程组含两个未知数,三个方程,方程数大于未知量数,通过牛顿迭代与最小二乘法求解。

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 仿真结果

利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第10张图片最终迭代次数为23次,小于100次,故仿真结果正确。
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第11张图片用户位置即伪距定位方程组的根为x=1,y=-2。
利用泰勒展开和牛顿迭代求解定位方程matlab仿真_第12张图片
此时误差为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.

你可能感兴趣的:(卫星定位与导航,通信,其他)