定初轨问题

最新推荐文章于 2024-09-09 09:47:53 发布

一只快活的柠檬精^^

最新推荐文章于 2024-09-09 09:47:53 发布

阅读量141

点赞数 4

文章标签：数据库 matlab

本文链接：https://blog.csdn.net/weixin_59551496/article/details/140394674

版权

用matlab写了一个初轨确定问题，用到了改进的Laplace方法，，用17个观测数据，迭代两三次精度就能达到十的负十几次方，理论上讲最少三组观测数据就可以定轨，大家可以试一下。

输入的是测站观测到的高度角和方位角，给地心坐标系的赤经赤纬也可以，注意转换细节即可，得到的是六个轨道根数。

clc,clear;
%%无量纲化km kg s
lngth=6378.137;
wght=5.965e+24;
tm=sqrt(6378137^3/(6.6743*5.965e+13));

%观测数据
A=[271.9361,275.1601,277.6250,281.7988,287.4667,295.4322,306.9108,323.9602,344.6457,6.0186,22.1247,33.0931,41.6469,47.3865,51.4400,54.6550,57.1686];
h=[6.7076,9.6311,11.6617,14.8009,18.5502,23.0017,27.9622,32.6148,34.5228,32.3749,27.8551,23.1380,18.3515,14.5154,11.4344,8.7339,6.4067];
A=A/180*pi;
h=h/180*pi;

%时间转化
t_scnd=[28470.9946,28500.9946,28518.9946,28542.9946,28566.9946,28590.9946,28614.9946,28639.9946,28663.9946,28688.9946,28712.9946,28735.9946,28761.9946,28786.9946,28810.9946,28835.9946,28860.9946];
dt=t_scnd-t_scnd(1);
t_jd=[];%转换成儒略日期
for i=1:17
    t_jd(i)=ltime(t_scnd(i));
end
SG=[];%格林尼治平恒星时
for i=1:17
    t=(t_jd(i)-2451545)/36525.0;
    SG(i)=mod((18.6973746+879000.0513367*t+0.093104/3600*t^2-(6.2/3600)*1e-6*t^3),24);
end
S=[];%地方平恒星时
for i=1:17
   S(i)=(mod((SG(i)+8),24))/24*2*pi;
end
%归一化量纲，并且使得dt的第一个分量为t2-t1
dt=(dt+30.9946)/tm;

%测站坐标，每一个观测数据由于时间的改变就会有一个测站坐标
hght=(4e-2)/lngth;
lam=deg2rad(120);
fai=deg2rad(36);
epp=1/298.257;
N2=cos(fai)^2+(1-epp)^2*sin(fai)^2;
N=1/sqrt(N2);
Xa=(N+hght)*cos(fai)*cos(lam);
Ya=(N+hght)*cos(fai)*sin(lam);
Za=(N*(1-epp)*(1-epp)+hght)*sin(fai);
Ra=[Xa;Ya;Za];
ra=[];
for i=1:17
   ra(:,i)=HG(t_scnd(i))'*Ra; 
end

%观测矢量
L=[];
for i=1:17
   L(:,i)=GR(t_scnd(i))'* LR(S(i),A(i),h(i));
end
F=ones(1,17);
G=dt;

i=1;
for j=1:3:51
    M(j,:)=[F(i)*L(3,i),0,-F(i)*L(1,i),G(i)*L(3,i),0,G(i)*L(1,i)];
    M(j+1,:)=[0,F(i)*L(3,i),-F(i)*L(2,i),0,G(i)*L(3,i),-G(i)*L(2,i)];
    M(j+2,:)=[F(i)*L(2,i),-F(i)*L(1,i),0,G(i)*L(2,i),-G(i)*L(1,i),0];
    i=i+1;
end

i=1;
for j=1:3:51
    b(j)=L(3,i)*ra(1,i)-L(1,i)*ra(3,i);
    b(j+1)=L(3,i)*ra(2,i)-L(2,i)*ra(3,i);
    b(j+2)=L(2,i)*ra(1,i)-L(1,i)*ra(2,i);
    i=i+1;
end

x=gaussianElimination(M'*M,M'*b');
r0=x(1:3,:);
r1=x(4:6,:);

for k=1:10
    %更新f，g
    for j=1:17
        F(j)=f(r0,r1,dt(j));
        G(j)=g(r0,r1,dt(j));
    end
    %更新M
    i=1;
    for j=1:3:51
        M(j,:)=[F(i)*L(3,i),0,-F(i)*L(1,i),G(i)*L(3,i),0,G(i)*L(1,i)];
        M(j+1,:)=[0,F(i)*L(3,i),-F(i)*L(2,i),0,G(i)*L(3,i),-G(i)*L(2,i)];
        M(j+2,:)=[F(i)*L(2,i),-F(i)*L(1,i),0,G(i)*L(2,i),-G(i)*L(1,i),0];
        i=i+1;
    end
    %更新b
    i=1;
    for j=1:3:51
        b(j)=L(3,i)*ra(1,i)-L(1,i)*ra(3,i);
        b(j+1)=L(3,i)*ra(2,i)-L(2,i)*ra(3,i);
        b(j+2)=L(2,i)*ra(1,i)-L(1,i)*ra(2,i);
        i=i+1;
    end
    x=gaussianElimination(M'*M,M'*b');
    e=norm(x-[r0;r1]);
    r0=x(1:3,:);
    r1=x(4:6,:);
end
y=orbit(r0,r1);

%单位观测矢量,S是地方平恒星时加测站8h，A是azimuth，h是altitude
function L=LR(S,A,h)
fai=deg2rad(36);
a=[cos(h)*cos(A);-cos(h)*sin(A);sin(h)];
L=Rz(pi-S)*Ry(pi/2-fai)*a;
end

%计算岁差章动矩阵等函数，参数为给定日期的第多少秒
function y=HG(s)
y=EP()*ER(s)*NR(s)*PR(s);
end

function y=GR(s)
y=NR(s)*PR(s);
end

function y = PR(s)
t=Time(s);
%计算儒略世纪数
ksa=2306.2181*t+0.30188*t^2+0.017998*t^3;
za=2306.2181*t+1.09468*t^2+0.018203*t^3;
cta=2004.3109*t-0.42665*t^2-0.041833*t^3;
%计算章动角
ksa=deg2rad(ksa);
za=deg2rad(za);
cta=deg2rad(cta);
y=Rz(-za)*Ry(cta)*Rz(-ksa);   
end

function y=NR(s)
t=Time(s);
ep0=23+26/60+21.448/3600;
epA=ep0-46.8150/3600*t;
epA=deg2rad(epA);
dfai=-8.3386e-5*sin(2.182-33.76*t);
dep=4.4615e-5*cos(2.182-33.76*t);
y=Rx(-(epA+dep))*Rz(-dfai)*Rx(epA);
end

function y=ER(s)
t=Time(s);
ep0=23+26/60+21.448/3600;
asg=18.6973746+879000.0513367*t+0.093104/3600*t*t-6.2e-6/3600*t*t*t;
asg=mod(asg,24)*15;
asg=deg2rad(asg);
epA=ep0-46.8150/3600*t;
epA=deg2rad(epA);
dfai=-8.3386e-5*sin(2.182-33.67*t);
dmu=dfai*cos(epA);
sg=asg+dmu;
y=Rz(sg);
end

function y=EP()
xp=deg2rad(-0.038212/3600);
yp=deg2rad(0.539719/3600);
y=Ry(-xp)*Rx(-yp);
end


function y = Rx(theta)
    y=[1,0,0;0,cos(theta),sin(theta);0,-sin(theta),cos(theta)];
end
function y = Ry(theta)
    y=[cos(theta),0,-sin(theta);0,1,0;sin(theta),0,cos(theta)];
end
function y = Rz(theta)
    y=[cos(theta),sin(theta),0;-sin(theta),cos(theta),0;0,0,1];
end

%计算给定时间的儒略世纪数，参数为给定日期的第多少秒
function y = Time(seconds)
    % 输入日期和时间
    hour = fix(seconds/3600);
    rseconds = mod(seconds,3600);
    min = fix(rseconds/60);
    second = mod(rseconds,60);

    % 构造日期时间对象的正确字符串格式
    tstring = sprintf('2002-03-30 %02d:%02d:%09.6f', hour, min, second);
    t = datetime(tstring, 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');

    % 计算儒略日期
    jd = juliandate(t);

    % 计算儒略日期的整数部分和小数部分
    jd_integer = floor(jd);
    jd_fraction = jd - jd_integer;

    % 计算儒略世纪数
    % 一个世纪有36525天
    % 使用2000年1月1日作为基准日期
    jd_2000 = juliandate(datetime(2000, 1, 1));
    julian_century = (jd - jd_2000) / 36525;

    % 返回结果（如果需要）
    y = julian_century;
end

% 计算儒略日期
function y=ltime(seconds)
% 输入日期和时间
    hour = fix(seconds/3600);
    rseconds = mod(seconds,3600);
    min = fix(rseconds/60);
    second = mod(rseconds,60);

    % 构造日期时间对象的正确字符串格式
    tstring = sprintf('2002-03-30 %02d:%02d:%09.6f', hour, min, second);
    t = datetime(tstring, 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');

    % 计算儒略日期
    y = juliandate(t);
end

%F和G，r是位置矢量，v是速度矢量，dt是时间与初始时间的差
function y=f(r,v,dt)
rv=dot(r,v);
rn=norm(r);
vn=norm(v);
y=1-u(3)*(dt^2/2)+(dt^3/2)*u(5)*rv+(dt^4/24)*u(5)*(3*vn^2-2*u(1)-15*u(2)*rv^2)+(dt^5/8)*u(7)*rv*(-3*vn^2+2*u(1)+7*u(2)*rv^2)+(dt^6/720)*u(7)*(u(2)*rv^2*(630*vn^2-420*u(1)-945*u(2)*rv^2)-(22*u(2)-66*u(1)*vn^2+45*vn^4));
function u=u(n)
u=1/rn^n;
end
end
function y=g(r,v,dt)
rv=dot(r,v);
rn=norm(r);
vn=norm(v);
y=dt-u(3)*(dt^3/6)+(dt^4/4)*u(5)*rv+(dt^5/120)*u(5)*(9*vn^2-8*u(1)-45*u(2)*rv^2)+(dt^6/24)*u(7)*rv*(-6*vn^2+5*u(1)+14*u(2)*rv^2)+(dt^6/24)*u(7)*rv*(-6*vn^2+5*u(1)+14*u(2)*rv^2);
function u=u(n)
u=1/rn^n;
end
end


%由r和v求六个轨道根数
function y=orbit(r,v)
mu=1;
rn=norm(r);
vn=norm(v);
rv=dot(r,v);
a=1/(2/rn-vn^2/mu);
e=sqrt((1-rn/a)^2+rv^2/(1*a));%%%%?
E=atan2(rv/sqrt(1/a),1-rn/a);%四象限反正切函数，返回给定坐标 (x, y) 所在点的角度，其范围通常是从 -π 到 π
if(E<0)
    E=E+2*pi;
end
M=E-e*sin(E);
if(M<0)
    M=M+2*pi;
end
P=cos(E)/rn*r-sqrt(a/mu)*sin(E)*v;
Q=(sin(E)/rn*r+sqrt(a/mu)*(cos(E)-e)*v)/sqrt(1-e^2);
R=1/sqrt(mu*a*(1-e^2))*cross(r,v);
k=sqrt(1/(1*a*(1-e^2)));
omg=atan2(P(3),Q(3));
if(omg<0)
    omg=omg+2*pi;
end
OMG=atan2(R(1),-R(2));
if(OMG<0)
    OMG=OMG+2*pi;
end
i=acos(R(3));
y=[a,e,i,omg,OMG,M];
end

function x = gaussianElimination(A, b)
    % 高斯消去法解线性方程组 Ax = b
    % 输入参数：
    % A: 系数矩阵
    % b: 右侧常数向量
    
    % 确认 A 是方阵
    [m, n] = size(A);
    if m ~= n
        error('系数矩阵 A 必须是一个方阵。');
    end
    
    % 合并增广矩阵
    Ab = [A b];
    
    % 消元过程
    for k = 1:n-1
        % 选主元
        [~, max_row] = max(abs(Ab(k:n, k)));
        max_row = max_row + k - 1;  % 转换为全局索引
        
        % 交换当前行和主元所在行
        if max_row ~= k
            Ab([k max_row], :) = Ab([max_row k], :);
        end
        
        % 消元
        for i = k+1:n
            factor = Ab(i, k) / Ab(k, k);
            Ab(i, :) = Ab(i, :) - factor * Ab(k, :);
        end
    end
    
    % 回代过程
    x = zeros(n, 1);
    for i = n:-1:1
        x(i) = (Ab(i, n+1) - Ab(i, 1:n) * x) / Ab(i, i);
    end
end