Почему моя расширенная реализация фильтрации Калмана по положению спутника не соответствует моим данным измерений, а сли ⇐ Python

[Anonymous]

Я реализовал расширенную фильтрацию Калмана на Python для фильтрации зашумленных данных измерений проекций широты и долготы Международной космической станции на Земле. Однако фильтр почти точно соответствует математической модели и вообще не пытается хорошо соответствовать моим зашумленным данным измерений. Несмотря на изменение значений P, Q и R в матрицах ковариации и шума, разницы почти нет, пока фильтр не начинает вести себя хаотично.
Я смоделировал математическую модель довольно точно. Это дает достойные результаты по сравнению с данными НАСА TLE. Поэтому я не думаю, что это проблема. Я считаю, что проблема заключается в моей расширенной реализации фильтра Калмана. Я думаю, что закодированные мной матрицы Якобиана также точны. Мой код имеет 7 векторов состояния. Положение в 3 координатах, скорость в 3 координатах и ​​большая полуось. В каждом цикле фильтра я обновляю эти значения и ввожу их в модель наблюдения, которая меняет значения на координаты широты и долготы. Остаток, который я получаю, выражен в широте и долготе и не выходит за пределы диапазона (-20, 20). Но независимо от того, как я меняю значения P, Q и R, я не вижу большой разницы в окончательных графиках, которые я прикрепил к вопросу. Изображение представляет собой построенный график зависимости долготы от широты в результатах измерения, модели и фильтра. Мой Python код:
import numpy as np
import matplotlib.pyplot as plt
from scipy.constants import G, pi
from skyfield.api import load
from datetime import timedelta

#Constants
mu = 3.986e14 # Gravitational parameter (m^3/s^2)
Re = 6378100 # Earth radius in m
Cd = 2.7 # Drag coefficient
A = 1996.91 # Cross-sectional area in m^2
mass = 460460 # Mass of the ISS in kg
earth_rotation_rate = 7.2921159e-5 # rad/s

# Orbital Elements
semi_major_axis = 6780000 # in m
eccentricity = 0.001
inclination = np.radians(51.64)
raan = np.radians(60)
true_anomaly = np.radians(0) # starting point (initial anomaly)

# Dynamic atmospheric density function based on altitude
def atmospheric_density(altitude):
# Approximate atmospheric density model (kg/m^3)
altitude = altitude / 1000
return 1.02e07 * altitude**-7.172

# Calculate drag and update semi-major axis
def calculate_drag(semi_major_axis, velocity, t):
altitude = semi_major_axis - Re
rho = atmospheric_density(401900)
# print(rho)
drag_acc = -0.5 * Cd * (A / mass) * rho * velocity**2
# Calculate the change in the semi-major axis using the drag formula
delta_a_dot = - (2 * (semi_major_axis)**2 / mu) * drag_acc * velocity * t

semi_major_axis_updated = semi_major_axis - delta_a_dot
return semi_major_axis_updated

def gravitational_acceleration(x, y, z):
r = np.sqrt(x**2 + y**2 + z**2)
g_x = -mu * x / r**3
g_y = -mu * y / r**3
g_z = -mu * z / r**3
return np.array([g_x, g_y, g_z])

def state_transition(state, delta_t):
x, y, z, v_x, v_y, v_z, a = state

# Define velocity and acceleration at current state
def derivatives(s):
x, y, z, v_x, v_y, v_z, _ = s
accel_gravity = gravitational_acceleration(x, y, z)
return np.array([
v_x, v_y, v_z,
accel_gravity[0], accel_gravity[1], accel_gravity[2],
0
])

# RK4 calculations
k1 = derivatives(state)
k2 = derivatives(state + 0.5 * delta_t * k1)
k3 = derivatives(state + 0.5 * delta_t * k2)
k4 = derivatives(state + delta_t * k3)

# Update state
new_state = state + (delta_t / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4)

# Update semi-major axis with drag
velocity = np.sqrt(new_state[3]**2 + new_state[4]**2 + new_state[5]**2)
new_state[6] = calculate_drag(new_state[6], velocity, delta_t)
r = np.sqrt(new_state[0]**2 + new_state[1]**2 + new_state[2]**2)
return new_state

def compute_F_jacobian(state, delta_t):
x, y, z, v_x, v_y, v_z, a = state
rho = atmospheric_density(401900)
# Initialize the Jacobian F
F = np.zeros((7, 7))

# Position derivatives
F[0, 0] = 1
F[0, 3] = (-mu*delta_t*(-2*x**2 + y**2 + z**2))/(np.sqrt((x**2+y**2+z**2)**5))
F[0, 4] = (3*mu*delta_t*x*y)/(np.sqrt((x**2+y**2+z**2)**5))
F[0, 5] = (3*mu*delta_t*x*z)/(np.sqrt((x**2+y**2+z**2)**5))
F[1, 1] = 1
F[1, 3] = (3*mu*delta_t*x*y)/(np.sqrt((x**2+y**2+z**2)**5))
F[1, 4] = (-mu*delta_t*(-2*y**2 + x**2 + z**2))/(np.sqrt((x**2+y**2+z**2)**5))
F[1, 5] = (3*mu*delta_t*y*z)/(np.sqrt((x**2+y**2+z**2)**5))
F[2, 2] = 1
F[2, 3] = (3*mu*delta_t*x*z)/(np.sqrt((x**2+y**2+z**2)**5))
F[2, 4] = (3*mu*delta_t*y*z)/(np.sqrt((x**2+y**2+z**2)**5))
F[2, 5] = (-mu*delta_t*(-2*z**2 + x**2 + y**2))/(np.sqrt((x**2+y**2+z**2)**5))

# Velocity derivatives
F[3, 0] = delta_t
F[3, 3] = 1 - Cd*(A/mass)*rho*v_x*delta_t
F[3, 6] = -3*(a**2/mu)*Cd*(A/mass)*rho*(v_x**2+v_y**2+v_z**2)**0.5*v_x*delta_t
F[4, 1] = delta_t
F[4, 4] = 1 - Cd*(A/mass)*rho*v_y*delta_t
F[4, 6] = -3*(a**2/mu)*Cd*(A/mass)*rho*(v_x**2+v_y**2+v_z**2)**0.5*v_y*delta_t
F[5, 2] = delta_t
F[5, 5] = 1 - Cd*(A/mass)*rho*v_z*delta_t
F[5, 6] = -3*(a**2/mu)*Cd*(A/mass)*rho*(v_x**2+v_y**2+v_z**2)**0.5*v_z*delta_t

# Semi-major axis update derivative
F[6, 6] = 1 - 2*(a/mu)*Cd*(A/mass)*rho*(v_x**2+v_y**2+v_z**2)**1.5*delta_t

return F

def observation_model(state, t):
x, y, z = state[0], state[1], state[2]

# Calculate radial distance from Earth's center
# Latitude (in degrees)
x_geo = (x * np.cos(raan) - y * np.sin(raan)) * np.cos(inclination)
y_geo = x * np.sin(raan) + y * np.cos(raan)
z_geo = y * np.sin(inclination)

lat = np.arcsin(z_geo / np.sqrt(x_geo**2 + y_geo**2 + z_geo**2)) * 180 / np.pi
lon = np.arctan2(y_geo, x_geo) * 180 / np.pi
lon -= earth_rotation_rate * t * 180 / np.pi # convert rad to degrees
return np.array([lat, lon])

def calculate_H_jacobian(state):
x, y, z = state[0:3]
H = np.zeros((2, 7))

H[0, 0] = -z * x / ((x**2 + y**2 + z**2) * np.sqrt(x**2 + y**2))
H[0, 1] = -z * y / ((x**2 + y**2 + z**2) * np.sqrt(x**2 + y**2))
H[0, 2] = np.sqrt(x**2 + y**2) / (x**2 + y**2 + z**2)

H[1, 0] = -y / (x**2 + y**2)
H[1, 1] = x / (x**2 + y**2)

return H

# Load TLE data for ISS from CelesTrak
stations_url = 'https://celestrak.com/NORAD/elements/stations.txt'
satellites = load.tle_file(stations_url)
by_name = {sat.name: sat for sat in satellites}

iss = by_name['ISS (ZARYA)']

# Define the timescale and custom time
ts = load.timescale()
t_custom = ts.utc(2024, 11, 9, 12, 54, 0)

times = []
latitudes_meas = []
longitudes_meas = []

# Measure the ISS's position every minute for the next 90 minutes
for i in range(90):
t2 = t_custom + timedelta(minutes=i)
geocentric = iss.at(t2)
subpoint = geocentric.subpoint()

latitude = subpoint.latitude.degrees
longitude = subpoint.longitude.degrees

# Store the data
times.append(t2.utc_strftime('%Y-%m-%d %H:%M:%S'))
latitudes_meas.append(latitude)
longitudes_meas.append(longitude)

noise_std = 2.5 # Noise level (standard deviation
noisy_latitudes = latitudes_meas + np.random.normal(0, noise_std, len(latitudes_meas))
noisy_longitudes = longitudes_meas + np.random.normal(0, noise_std, len(longitudes_meas))

# Initialize a state vector with semi-major axis
initial_state = np.array([
5429.128332332650 * 1000, # Initial x (m)
-975.311960132481 * 1000, # Initial y (m)
3957.346410376780 * 1000, # Initial z (m)
-1.80166995818100 * 1000, # Initial x-velocity (m/s)
6.28473745611627 * 1000, # Initial y-velocity for circular orbit (m/s)
3.99918311510884 * 1000, # Initial z-velocity (m/s)
6780 * 1000 # Semi-major axis (m)
])
delta_t = 60
time_steps = 90 * 60 // delta_t
results = np.zeros((time_steps, 7))
ttime = 0
ttimeobs = 0
observed_values = []
P = np.diag([2e3, 2e3, 2e3, 1e0, 1e0, 1e0, 2e0])
Q = np.diag([2e3, 3e3, 3e3, 2e0, 3e0, 2e0, 2e0])
R = np.diag([noise_std, noise_std])
current_state = initial_state

for i in range(time_steps) :
ttimeobs += i
current_state = state_transition(current_state, delta_t) # Update state with refined delta_t
# print(current_state)
true_measurements = observation_model(current_state, ttimeobs)
observed_values.append(true_measurements)

lat_modeled = []
long_modeled = []

for i in observed_values:
lat_modeled.append(i[0])
long_modeled.append(i[1])

for i in range(time_steps):
ttime += i
results = current_state
# print(results)
# Prediction Stage
current_state = state_transition(current_state, delta_t) # Update state with refined delta_t
F_jacobian = compute_F_jacobian(current_state, delta_t)
P = F_jacobian @ P @ np.transpose(F_jacobian) + Q
# print(P[0])

# Update Stage
observation = observation_model(current_state, ttime)

z = np.array([noisy_latitudes, noisy_longitudes])
residual = z - observation
print(i, residual, observation[1], z[1])
H_jacobian = calculate_H_jacobian(current_state)
S = H_jacobian @ P @ np.transpose(H_jacobian) + R
K = P @ np.transpose(H_jacobian) @ np.linalg.inv(S)
# print(K)
current_state = current_state + K @ residual # Updated state estimate
P = (np.eye(len(current_state)) - K @ H_jacobian) @ P # Updated covariance estimate

lat_filtered = []
long_filtered = []
ttime = 0

for i in range(len(results)):
ttime += i
observation = observation_model(results, ttime)
lat_filtered.append(observation[0])
long_filtered.append(observation[1])

#Plotting longitude vs latitude
plt.figure(figsize=(10, 5))
plt.plot(noisy_longitudes, noisy_latitudes, 'o-', color='blue', markersize=5)
plt.plot(long_filtered, lat_filtered, 'o-', color='green', markersize=5)
plt.plot(long_modeled, lat_modeled, 'o-', label='Mathematical Model (with perturbations)', color='red', markersize=5)
plt.title('Longitude vs Latitude')
plt.xlabel('Longitude (degrees)')
plt.ylabel('Latitude (degrees)')
plt.grid()
plt.tight_layout()
plt.show()


Подробнее здесь: https://stackoverflow.com/questions/791 ... on-not-fit