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

实验原理及代码讲解链接: https://blog.csdn.net/qq_44394952/article/details/122259103.

position_main.m伪距定位算法

clear;
clc;
c=299792.458;  %光速,km/s
%通过星历参数解算到所在地可见卫星的坐标位置
sat1=[4 2];
sat2=[3 5];
sat3=[-3 2];
sat=[sat1;sat2;sat3];
%多卫星位置矩阵
r=[5;sqrt(53);sqrt(32)];
%理想伪距测量值
xyt=[0 0 0];
%牛顿迭代法迭代初值全部设为 0
for i = 1:100 
%最大迭代次数设为 100 次
 f = position_f(xyt,sat,r); 
%定位方程函数
 df = position_df(xyt,sat); 
%偏导数矩阵
 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
disp('迭代次数为:') 
i
disp('用户位置为:')
xyt(1:2)
disp('误差为:') 
P

position_f.m多卫星定位方程函数

function f=position_f(xyt,sat,r)
%xyt(1:2)为用户位置(km)
%xyt(3)为用户钟差(s)
%r 为测量得到的伪距(km)
%sat 为卫星数据
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';

position_df.m多颗卫星定位的偏导数矩阵

function df=position_df(xyt,sat)
%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