人工衛星の姿勢と座標系 — 軌道座標系から機体座標系まで

import numpy as np

# 定数
MU_EARTH = 3.986004418e14  # 地球の重力パラメータ [m^3/s^2]
R_EARTH = 6.371e6           # 地球の平均半径 [m]
OMEGA_EARTH = 7.2921159e-5  # 地球の自転角速度 [rad/s]

def eci_to_ecef(r_eci, gmst):
    """ECI座標からECEF座標への変換"""
    Rz = np.array([
        [ np.cos(gmst), np.sin(gmst), 0],
        [-np.sin(gmst), np.cos(gmst), 0],
        [0,             0,            1]
    ])
    return Rz @ r_eci

def compute_lvlh_dcm(r_eci, v_eci):
    """ECI位置・速度ベクトルからLVLH座標系の回転行列を計算"""
    r = r_eci / np.linalg.norm(r_eci)
    h = np.cross(r_eci, v_eci)
    h = h / np.linalg.norm(h)

    z_o = -r            # ナディア方向
    y_o = -h            # 軌道面法線の逆方向
    x_o = np.cross(y_o, z_o)
    x_o = x_o / np.linalg.norm(x_o)

    # 回転行列（各行がLVLH軸のECI成分）
    C_OI = np.vstack([x_o, y_o, z_o])
    return C_OI

def euler_to_dcm_321(phi, theta, psi):
    """3-2-1オイラー角から回転行列"""
    Rx = np.array([[1,0,0],[0,np.cos(phi),-np.sin(phi)],[0,np.sin(phi),np.cos(phi)]])
    Ry = np.array([[np.cos(theta),0,np.sin(theta)],[0,1,0],[-np.sin(theta),0,np.cos(theta)]])
    Rz = np.array([[np.cos(psi),-np.sin(psi),0],[np.sin(psi),np.cos(psi),0],[0,0,1]])
    return Rx @ Ry @ Rz

# 円軌道の例（高度500km、軌道傾斜角45度）
alt = 500e3  # 高度 [m]
a = R_EARTH + alt
inc = np.radians(45)  # 軌道傾斜角
nu = np.radians(30)   # 真近点角

# 軌道面内のECI位置・速度
v_orbital = np.sqrt(MU_EARTH / a)
r_orb = a * np.array([np.cos(nu), np.sin(nu), 0])
v_orb = v_orbital * np.array([-np.sin(nu), np.cos(nu), 0])

# 軌道傾斜角の適用（x軸回りに傾ける、簡略化）
Rx_inc = np.array([
    [1, 0, 0],
    [0, np.cos(inc), -np.sin(inc)],
    [0, np.sin(inc),  np.cos(inc)]
])
r_eci = Rx_inc @ r_orb
v_eci = Rx_inc @ v_orb

# LVLH座標系の計算
C_OI = compute_lvlh_dcm(r_eci, v_eci)
print("LVLH rotation matrix (C_OI):")
print(np.round(C_OI, 6))
print(f"\nDet(C_OI) = {np.linalg.det(C_OI):.10f}")
print(f"C_OI @ C_OI.T =\n{np.round(C_OI @ C_OI.T, 10)}")

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

MU_EARTH = 3.986004418e14
R_EARTH = 6.371e6

def compute_lvlh_dcm(r_eci, v_eci):
    r = r_eci / np.linalg.norm(r_eci)
    h = np.cross(r_eci, v_eci)
    h = h / np.linalg.norm(h)
    z_o = -r
    y_o = -h
    x_o = np.cross(y_o, z_o)
    x_o = x_o / np.linalg.norm(x_o)
    return np.vstack([x_o, y_o, z_o])

# 軌道パラメータ
alt = 500e3
a = R_EARTH + alt
inc = np.radians(45)
v_orbital = np.sqrt(MU_EARTH / a)

Rx_inc = np.array([
    [1, 0, 0],
    [0, np.cos(inc), -np.sin(inc)],
    [0, np.sin(inc),  np.cos(inc)]
])

# 軌道を1周分計算
nu_arr = np.linspace(0, 2*np.pi, 200)
orbit_eci = np.zeros((len(nu_arr), 3))
for i, nu in enumerate(nu_arr):
    r_orb = a * np.array([np.cos(nu), np.sin(nu), 0])
    orbit_eci[i] = Rx_inc @ r_orb

# 地球表面（簡易球体）
u = np.linspace(0, 2*np.pi, 40)
v = np.linspace(0, np.pi, 20)
xe = R_EARTH * np.outer(np.cos(u), np.sin(v))
ye = R_EARTH * np.outer(np.sin(u), np.sin(v))
ze = R_EARTH * np.outer(np.ones(np.size(u)), np.cos(v))

fig = plt.figure(figsize=(16, 7))

# === 左: ECI座標系と軌道 ===
ax1 = fig.add_subplot(121, projection='3d')
ax1.plot_surface(xe, ye, ze, alpha=0.15, color='skyblue')
ax1.plot(orbit_eci[:,0], orbit_eci[:,1], orbit_eci[:,2],
         'b-', linewidth=1.5, label='Orbit')

# ECI軸
scale = R_EARTH * 1.8
for i, (c, lbl) in enumerate(zip(['r','g','b'],
                                  ['$x_{ECI}$ (Vernal eq.)','$y_{ECI}$','$z_{ECI}$ (North)'])):
    d = np.zeros(3); d[i] = scale
    ax1.quiver(0,0,0,*d, color=c, arrow_length_ratio=0.05, linewidth=2)
    ax1.text(*(d*1.1), lbl, color=c, fontsize=9)

# 衛星位置（nu=60度）
nu_sat = np.radians(60)
r_sat_orb = a * np.array([np.cos(nu_sat), np.sin(nu_sat), 0])
v_sat_orb = v_orbital * np.array([-np.sin(nu_sat), np.cos(nu_sat), 0])
r_sat = Rx_inc @ r_sat_orb
v_sat = Rx_inc @ v_sat_orb
ax1.scatter(*r_sat, c='orange', s=80, zorder=5, label='Satellite')

ax1.set_title('ECI Frame & Satellite Orbit', fontsize=13)
ax1.set_xlabel('X [m]'); ax1.set_ylabel('Y [m]'); ax1.set_zlabel('Z [m]')
ax1.legend(fontsize=9, loc='upper left')

# === 右: 衛星位置でのLVLH座標系 ===
ax2 = fig.add_subplot(122, projection='3d')

# 軌道（スケーリング）
s = 1e-6  # mからMmへ
ax2.plot(orbit_eci[:,0]*s, orbit_eci[:,1]*s, orbit_eci[:,2]*s,
         'b-', linewidth=1, alpha=0.5)
ax2.plot_surface(xe*s, ye*s, ze*s, alpha=0.1, color='skyblue')

# 衛星位置
ax2.scatter(*(r_sat*s), c='orange', s=100, zorder=5)

# LVLH軸
C_OI = compute_lvlh_dcm(r_sat, v_sat)
lvlh_scale = R_EARTH * 0.5 * s
labels_o = [r'$x_o$ (along-track)', r'$y_o$ (cross-track)', r'$z_o$ (nadir)']
colors_o = ['#FF5722', '#4CAF50', '#2196F3']
for i in range(3):
    d = C_OI[i] * lvlh_scale  # LVLH軸のECI方向
    ax2.quiver(*(r_sat*s), *d, color=colors_o[i],
               arrow_length_ratio=0.08, linewidth=2.5)
    ax2.text(*(r_sat*s + d*1.2), labels_o[i], color=colors_o[i], fontsize=9)

# ナディアライン（衛星→地球中心）
ax2.plot([r_sat[0]*s, 0], [r_sat[1]*s, 0], [r_sat[2]*s, 0],
         'k--', linewidth=0.8, alpha=0.4)

ax2.set_title('LVLH Frame at Satellite Position', fontsize=13)
ax2.set_xlabel('X [Mm]'); ax2.set_ylabel('Y [Mm]'); ax2.set_zlabel('Z [Mm]')

plt.tight_layout()
plt.savefig('satellite_coordinate_systems.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def euler_to_dcm_321(phi, theta, psi):
    Rx = np.array([[1,0,0],[0,np.cos(phi),-np.sin(phi)],[0,np.sin(phi),np.cos(phi)]])
    Ry = np.array([[np.cos(theta),0,np.sin(theta)],[0,1,0],[-np.sin(theta),0,np.cos(theta)]])
    Rz = np.array([[np.cos(psi),-np.sin(psi),0],[np.sin(psi),np.cos(psi),0],[0,0,1]])
    return Rx @ Ry @ Rz

def plot_frame(ax, R, origin, length, labels, colors, alpha=1.0, fontsize=9):
    for i in range(3):
        d = R[:, i] * length
        ax.quiver(*origin, *d, color=colors[i],
                  arrow_length_ratio=0.08, linewidth=2.5, alpha=alpha)
        ax.text(*(origin + d*1.15), labels[i], color=colors[i],
                fontsize=fontsize, alpha=alpha)

fig = plt.figure(figsize=(16, 5))
cases = [
    (15, 0, 0, 'Roll φ=15°'),
    (0, 15, 0, 'Pitch θ=15°'),
    (0, 0, 15, 'Yaw ψ=15°')
]

lvlh_labels = [r'$x_o$', r'$y_o$', r'$z_o$']
body_labels = [r'$x_b$', r'$y_b$', r'$z_b$']
lvlh_colors = ['#FFCDD2', '#C8E6C9', '#BBDEFB']  # 薄い色
body_colors = ['#D32F2F', '#388E3C', '#1976D2']   # 濃い色

for idx, (phi_d, theta_d, psi_d, title) in enumerate(cases):
    ax = fig.add_subplot(1, 3, idx+1, projection='3d')

    # LVLH座標系（基準、薄い色）
    plot_frame(ax, np.eye(3), np.zeros(3), 1.0,
               lvlh_labels, lvlh_colors, alpha=0.5)

    # 機体座標系（姿勢回転後）
    C_BO = euler_to_dcm_321(np.radians(phi_d), np.radians(theta_d), np.radians(psi_d))
    plot_frame(ax, C_BO.T, np.zeros(3), 1.0,
               body_labels, body_colors)

    ax.set_title(title, fontsize=13)
    ax.set_xlim([-1.3,1.3]); ax.set_ylim([-1.3,1.3]); ax.set_zlim([-1.3,1.3])
    ax.set_xlabel('Along-track'); ax.set_ylabel('Cross-track')
    ax.set_zlabel('Nadir')

plt.suptitle('Satellite Attitude Angles: Body Frame (dark) vs LVLH Frame (light)',
             fontsize=14, y=1.02)
plt.tight_layout()
plt.savefig('attitude_angles.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

MU_EARTH = 3.986004418e14
R_EARTH = 6.371e6

def compute_lvlh_dcm(r_eci, v_eci):
    r = r_eci / np.linalg.norm(r_eci)
    h = np.cross(r_eci, v_eci)
    h = h / np.linalg.norm(h)
    z_o = -r
    y_o = -h
    x_o = np.cross(y_o, z_o)
    x_o = x_o / np.linalg.norm(x_o)
    return np.vstack([x_o, y_o, z_o])

alt = 500e3
a = R_EARTH + alt
inc = np.radians(45)
v_orbital = np.sqrt(MU_EARTH / a)

Rx_inc = np.array([
    [1, 0, 0],
    [0, np.cos(inc), -np.sin(inc)],
    [0, np.sin(inc),  np.cos(inc)]
])

fig = plt.figure(figsize=(14, 10))
ax = fig.add_subplot(111, projection='3d')

# 地球
u = np.linspace(0, 2*np.pi, 40)
v = np.linspace(0, np.pi, 20)
s = 1e-6
xe = R_EARTH * s * np.outer(np.cos(u), np.sin(v))
ye = R_EARTH * s * np.outer(np.sin(u), np.sin(v))
ze = R_EARTH * s * np.outer(np.ones(np.size(u)), np.cos(v))
ax.plot_surface(xe, ye, ze, alpha=0.12, color='skyblue')

# 軌道
nu_orbit = np.linspace(0, 2*np.pi, 300)
orbit = np.zeros((len(nu_orbit), 3))
for i, nu in enumerate(nu_orbit):
    r_orb = a * np.array([np.cos(nu), np.sin(nu), 0])
    orbit[i] = Rx_inc @ r_orb
ax.plot(orbit[:,0]*s, orbit[:,1]*s, orbit[:,2]*s,
        'b-', linewidth=1, alpha=0.5)

# 8つの位置でLVLH座標系を表示
nu_samples = np.linspace(0, 2*np.pi, 9)[:-1]  # 0から315度まで
colors_axes = ['#D32F2F', '#388E3C', '#1976D2']

for nu in nu_samples:
    r_orb = a * np.array([np.cos(nu), np.sin(nu), 0])
    v_orb = v_orbital * np.array([-np.sin(nu), np.cos(nu), 0])
    r_sat = Rx_inc @ r_orb
    v_sat = Rx_inc @ v_orb

    C_OI = compute_lvlh_dcm(r_sat, v_sat)
    frame_scale = R_EARTH * 0.3 * s

    for i in range(3):
        d = C_OI[i] * frame_scale
        ax.quiver(*(r_sat*s), *d, color=colors_axes[i],
                  arrow_length_ratio=0.12, linewidth=1.5)

    ax.scatter(*(r_sat*s), c='orange', s=30, zorder=5)

ax.set_xlabel('X [Mm]', fontsize=11)
ax.set_ylabel('Y [Mm]', fontsize=11)
ax.set_zlabel('Z [Mm]', fontsize=11)
ax.set_title('LVLH Frame Evolution Along Orbit\n'
             '(red: along-track, green: cross-track, blue: nadir)',
             fontsize=13)
plt.tight_layout()
plt.savefig('lvlh_orbit_evolution.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt

R_EARTH = 6.371e6
MU_EARTH = 3.986004418e14

# 高度範囲
alt_km = np.linspace(200, 2000, 500)
alt_m = alt_km * 1e3
R = R_EARTH + alt_m

# 衛星パラメータ（典型的な中型衛星）
mass = 500  # kg
A_cross = 2.0  # 断面積 [m^2]
A_solar = 5.0  # 太陽に面した面積 [m^2]
delta_I = 50   # |Iz - Ix| [kg m^2]
l_cp = 0.05    # 圧力中心オフセット [m]
l_sp = 0.1     # 太陽輻射圧中心オフセット [m]
m_residual = 0.5  # 残留磁気モーメント [Am^2]
sin_2alpha = 0.1  # 典型的な姿勢角による因子

# 重力傾斜トルク
T_gg = 3 * MU_EARTH / R**3 * delta_I * sin_2alpha

# 空気抵抗トルク（指数大気モデル近似）
rho_0 = 1.225  # 海面大気密度 [kg/m^3]
H = 8500  # スケールハイト [m]（低軌道用簡易モデル）
# より現実的なモデル: 高度500kmで約1e-12 kg/m^3
rho = 1e-10 * np.exp(-(alt_m - 300e3) / 50e3)
rho = np.clip(rho, 1e-16, None)
v_orb = np.sqrt(MU_EARTH / R)
Cd = 2.2
T_aero = 0.5 * rho * v_orb**2 * Cd * A_cross * l_cp

# 太陽輻射圧トルク
P_solar = 4.56e-6  # 太陽輻射圧 [N/m^2]
rho_ref = 0.5  # 反射率
T_srp = P_solar * A_solar * l_sp * (1 + rho_ref) * np.ones_like(alt_km)

# 残留磁気トルク（地球磁場のダイポールモデル）
B_0 = 3.12e-5  # 赤道表面での磁場 [T]
B = B_0 * (R_EARTH / R)**3
T_mag = m_residual * B

fig, ax = plt.subplots(figsize=(10, 7))
ax.semilogy(alt_km, T_gg, 'b-', linewidth=2, label='Gravity gradient')
ax.semilogy(alt_km, T_aero, 'r-', linewidth=2, label='Aerodynamic')
ax.semilogy(alt_km, T_srp, 'orange', linewidth=2, label='Solar radiation pressure')
ax.semilogy(alt_km, T_mag, 'g-', linewidth=2, label='Residual magnetic')

ax.set_xlabel('Altitude [km]', fontsize=13)
ax.set_ylabel('Disturbance Torque [Nm]', fontsize=13)
ax.set_title('Disturbance Torques vs Altitude (Typical LEO Satellite)', fontsize=14)
ax.legend(fontsize=12)
ax.grid(True, alpha=0.3, which='both')
ax.set_xlim([200, 2000])
ax.set_ylim([1e-10, 1e-3])

plt.tight_layout()
plt.savefig('disturbance_torques.png', dpi=150, bbox_inches='tight')
plt.show()