我在这段代码中对两个数组进行叉积时遇到问题,该代码应该计算卫星的姿态确定算法。
代码如下:
rom os import confstr_names
from pickleshare import print_function
import math
import numpy as np
np.set_printoptions(precision=15, suppress=True)
h = 422.0;
Lat = 48.5;
Long = -110.4;
Y = 2022;
M = 10;
D = 18;
hr = 11;
m = 50;
s = 2;
psi = 0;
theta = -90;
phi = 0;
TargetLat = 50.8;
TargetLong = -105.4;
Rearth = 6378.137;
Lat_rads = math.radians(Lat)
Long_rads = math.radians(Long)
psi_rads = math.radians(psi)
theta_rads = math.radians(theta)
phi_rads = math.radians(phi)
x = (Rearth + h) * math.cos(Lat_rads) * math.cos(Long_rads)
y = (Rearth + h) * math.cos(Lat_rads) * math.sin(Long_rads)
z = (Rearth + h) * math.sin(Lat_rads)
r = math.sqrt((x**2)+(y**2)+(z**2))
def Magnetic_field(x,y,z,r):
g1 = -1669.05
h1 = 5077.99
g0 = -29554.63
a = 6371.2
mx = (a**3)*g1
my = (a**3)*h1
mz = (a**3)*g0
Bx = (3*((mx*x) + (my*y) + (mz*z))*x - (r**2)*(mx))/(r**5)
By = (3*((mx*x) + (my*y) + (mz*z))*y - (r**2)*(my))/(r**5)
Bz = (3*((mx*x) + (my*y) + (mz*z))*z - (r**2)*(mz))/(r**5)
B1 = math.sqrt((Bx**2) + (By**2) + (Bz**2))
B2 = np.array([[Bx], [By], [Bz]])
return B2;
B2 = Magnetic_field(x,y,z,r)
print("B = ",B2)
def Sun_Position(Y, M, D, hr, m, s, x, y, z):
b1 = (7/4) * (Y+1)
b2 = round(b1)
c1 = (275 * M)/9
c2 = round(c1)
Au = 149597870.691
JD = 1721013.5 + 367*Y - b2 + c2 + D + (60*hr+m+s/60)/1440;
Tu = (JD-2451545)/36525;
Phi = 280.460 + 36000.771*Tu;
Mo = 357.5277233 + 35999.05034*Tu;
Phi_rads = math.radians(Phi)
Mo_rads = math.radians(Mo)
Phie = Phi + (1.914666471 * math.sin(Mo_rads)) + (0.019994643 * math.sin(2*Mo_rads))
Epsilon = 23.439291 - 0.0120042*Tu
Phie_rads = math.radians(Phie)
Epsilon_rads = math.radians(Epsilon)
eo = np.array([[(math.cos(Phie_rads))], [(math.cos(Epsilon_rads) * math.sin(Phie_rads))], [(math.sin(Epsilon_rads))*(math.sin(Phie_rads))]])
r1 = 1.000140612 - 0.016708617 * math.cos(Mo_rads) - 0.000139589 * math.cos(2*Mo_rads);
R1 = r1 * Au
RA = R1 * eo
RB1 = (RA[0])-x
RC1 = (RA[1])-y
RD1 = (RA[2])-z
R2 = np.sqrt((RB1**2) + (RC1**2) + (RD1**2))
e1 = RB1 / R2
e2 = RC1 / R2
e3 = RD1 / R2
Sun = np.array([e1, e2, e3])
return Sun;
Sun= Sun_Position(Y, M, D, hr, m, s, x, y, z)
print("Sun",Sun)
def Attitude_Matrix(psi_rads, theta_rads, phi_rads):
A = np.array([[math.cos(theta_rads)*math.cos(phi_rads), math.cos(theta_rads)*math.sin(phi_rads), -math.sin(theta_rads)], [(-math.cos(psi_rads)*math.sin(phi_rads))+(math.sin(psi_rads)*math.sin(theta_rads)*math.cos(phi_rads)), (math.cos(psi_rads)*math.cos(phi_rads)) + (math.sin(psi_rads)*math.sin(theta_rads)*math.cos(phi_rads)), math.sin(psi_rads)*math.cos(theta_rads)], [(math.cos(psi_rads)*math.sin(theta_rads)*math.cos(phi_rads)) + (math.sin(psi_rads)*math.sin(phi_rads)), ((math.cos(psi_rads)*math.sin(theta_rads)*math.sin(phi_rads)) - (math.sin(psi_rads)*math.cos(phi_rads))), (math.cos(psi_rads)*math.cos(theta_rads))]])
return A;
A = Attitude_Matrix(psi_rads, theta_rads, phi_rads)
print("Attitude Matrix = ",A)
r1 = B2/(np.linalg.norm(B2))
print("r1 = ",r1)
r2 = Sun/(np.linalg.norm(Sun))
print("r2 = ",r2)
b1 = A*r1.T
print("b1 = ",b1)
b1x = b1[0][-1] + ((0.2)*b1[0][-1])
b1y = b1[1][1] + ((0.1)*b1[1][1])
b1z = b1[2][0] + ((0.3)*b1[2][0])
b1error = [b1x, b1y, b1z]
print("b1error = ",b1error)
b2 = A*r2.T
print("b2 = ",b2)
b2x = b2[0][-1] + ((0.2)*b2[0][-1])
b2y = b2[1][1] + ((0.1)*b2[1][1])
b2z = b2[2][0] + ((0.3)*b2[2][0])
b2error = [b2x, b2y, b2z]
b1errornorm = b1error/(np.linalg.norm(b1error))
print("b1error normalized = ",b1errornorm)
b2errornorm = b2error/(np.linalg.norm(b2error))
print("b2error normalized = ",b2errornorm)
j1 = np.cross(b1errornorm , b2errornorm)
j1norm = j1/(np.linalg.norm(j1))
k = np.cross(b1errornorm,j1norm)
knorm = k/(np.linalg.norm(k))
L = np.array([b1errornorm, j1norm, knorm]).T
print("L = ",L)
r12 = np.array([r1]).T
r22= np.array([r2]).T
M = np.cross(r12, r22)
Mnorm = M/(np.linalg.norm(M))
print("Mnorm = ",Mnorm)
N = np.cross(r1 , M)
Nnorm = N/(np.linalg.norm(N))
O = np.array([[r1], [Mnorm], [Nnorm]]).T
TRIAD = L * O
错误信息是这样给出的:
---------------------------------------------------------------------------
ValueError Traceback (most recent call last)
<ipython-input-12-29ce1cf02cca> in <module>
114 r12 = np.array([r1]).T
115 r22= np.array([r2]).T
--> 116 M = np.cross(r12, r22)
117 Mnorm = M/(np.linalg.norm(M))
118 print("Mnorm = ",Mnorm)
1 frames
/usr/local/lib/python3.9/dist-packages/numpy/core/overrides.py in cross(*args, **kwargs)
/usr/local/lib/python3.9/dist-packages/numpy/core/numeric.py in cross(a, b, axisa, axisb, axisc, axis)
1605 "(dimension must be 2 or 3)")
1606 if a.shape[-1] not in (2, 3) or b.shape[-1] not in (2, 3):
-> 1607 raise ValueError(msg)
1608
1609 # Create the output array
ValueError: incompatible dimensions for cross product
(dimension must be 2 or 3)
我曾尝试将 r1 和 r2 直接转换为数组,但这会导致代码的 b1 和 b2 部分出现类似问题。我期待 r1 和 r2 的叉积,它们应该是代表矩阵的数组,每个矩阵有 1 列。