我在Python上实现了扩展卡尔曼滤波，以过滤国际空间站在地球上的纬度和经度投影的噪声测量数据。然而，滤波器几乎完全符合数学模型，并且根本没有尝试很好地拟合我的噪声测量数据。尽管改变了我的协方差和噪声矩阵的 P、Q 和 R 值，但在滤波器开始表现不稳定之前，几乎没有差异。
我以相当准确的方式对数学模型进行了建模。与 NASA 的 TLE 数据相比，它产生了不错的结果。因此我认为这不是问题所在。我相信问题在于我的扩展卡尔曼滤波器的实现。我认为我编码的雅可比矩阵也是准确的。我的代码有 7 个状态向量。 3 个坐标中的位置、3 个坐标中的速度和半长轴。在过滤器的每个循环中，我更新这些值并将它们输入到observations_model中，该模型将值更改为纬度和经度坐标。我得到的残差是纬度和经度的，并且不超过(-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[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()
在您共享的代码中，初始位置不确定性的标准偏差约为 45 米 (=sqrt(2*10^3))。每个步骤的过程噪声都是相同的数量级。因此，仅从模型来看，滤波器位置的不确定性约为数十或数百米。
您添加的测量噪声在纬度和经度上的标准偏差为 2.5 度。让我们快速计算一下。国际空间站轨道半径约为6778000米。我们可以使用弧长公式求出以米为单位的不确定度：s = r * theta = (6778000) * (2.5 * pi / 180) = 295746。因此，每次测量的不确定度约为数十万米。
当滤波器知道航天器的位置精确到几十米，然后得到的测量结果只能精确到几十万米时，它会做什么？正确的是，它几乎完全忽略测量结果。如果您希望滤波器关注测量结果，那么我建议将测量噪声“大幅”降低。例如，如果您希望测量结果具有 100 米的不确定度，则 0.0008 度的噪声标准偏差大约就是您想要的。