I implemented extended kalman filtering on python to filter noisy measurement data of the International Space Station's latitude and longitude projections on earth. However, the filter almost fits the mathematical model exactly and doesn't try to fit my noisy measurement data well at all. Despite changing values of P, Q and R my covariance and noise matrices, there is almost no difference until the filter starts behaving erratically.
I modeled the math model in a fairly accurate way. It yields decent results as compared to NASA's TLE data. Therefore I don't think that is the issue. I believe the issue lies in my extended kalman filter implementation. I think the jacobian matrices I coded are also accurate. My code has 7 state vectors. Position in 3 coordinates, velocity in 3 coordinates and the semi major axis. In each loop of the filter I update those values and enter them in an observation_model that changes the values to latitude and longitude coordinates. The residual I get is in latitudes and longitudes and doesn't exceed the range of (-20, 20). But no matter how I change the P, Q and R values, I dont see a big difference in the final graphs I have attached to the question.The image is the plotted graph of longitude vs latitude in measurement, model and filter outputs My python code:
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[i] = current_state
# print(results[i])
# 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[i], noisy_longitudes[i]])
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[i], 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()
In the code you shared, the initial position uncertainties have a standard deviation of about 45 meters (=sqrt(2*10^3)). The process noise at each step is the same order of magnitude. So the filter's uncertainty in its position just from the model is on the order of tens or low hundreds of meters.
The measurement noise that you are adding has a standard deviation of 2.5 degrees in latitude and longitude. Let's do some quick math on that. The radius of the ISS orbit is about 6778000 meters. We can find the uncertainty in meters using the arc length formula: s = r * theta = (6778000) * (2.5 * pi / 180) = 295746. So the uncertainty in each measurement is on the order of hundreds of thousands of meters.
What will the filter do when it knows the position of the spacecraft accurate to tens of meters and then gets a measurement that's only accurate to hundreds of thousands of meters? It will, correctly, almost completely ignore the measurement. If you want the filter to pay attention to the measurements, then I would suggest turning the measurement noise way down. For example, if you wanted the measurements to have an uncertainty of 100 meters, a noise standard deviation of 0.0008 degrees is about what you would want.