sage-huga改进SITAN

Sage-Husa自适应滤波算法

Sage-Husa自适应滤波算法是一种在递推滤波过程中实时估计和修正系统噪声和观测噪声统计特性的算法，从而降低系统模型误差，提高滤波精度。该算法基于卡尔曼滤波，并通过自适应调整噪声协方差矩阵来优化滤波效果。

算法原理

Sage-Husa滤波器的核心思想是通过最大似然估计和自适应因子来动态调整噪声协方差矩阵。具体来说，算法通过以下步骤实现：

系统状态方程和观测方程：状态方程：xk = Akxk−1 + wk 观测方程：zk = Hkxk + vk 其中，Ak和Hk分别为状态转移矩阵和观测矩阵，wk和vk为过程噪声和观测噪声，其协方差矩阵分别为Qk和Rk¹。
卡尔曼滤波更新过程：预测状态：xk = Akxk−1 预测协方差：Pk = AkPk−1AkT + Qk 卡尔曼增益：Kk = PkHkT(HkPkHkT + Rk)−1 更新状态：xk = xk + Kk(zk − Hkxk) 更新协方差：Pk = (I − KkHk)Pk¹。
自适应调整噪声协方差矩阵：基于新息的观测噪声协方差矩阵（IAE）：通过新息向量vk = zk − Hkxk估计Rk¹。基于残差的观测噪声协方差矩阵（RAE）：通过残差向量v^k = zk − Hkx^k估计Rk1^。系统状态噪声协方差矩阵：通过状态误差Δxk = xk − xk−1估计Qk¹。

代码示例

以下是一个Sage-Husa自适应滤波算法的Python实现示例：

import numpy as np

def sage_husa_kf(F, G, H, Q0, R0, X0, Z, P0, b, s):
N = len(Z)
M = len(X0)
X = np.zeros((M, N))
X[:, 0] = X0
P = P0
q = np.zeros(M)
r = 0
Q = Q0
R = R0

for k in range(1, N):
X_est = F @ X[:, k-1] + q
P_pre = F @ P @ F.T + G @ Q @ G.T
e = Z[:, k] - H @ X_est - r
K = P_pre @ H.T @ np.linalg.inv(H @ P_pre @ H.T + R)
X[:, k] = X_est + K @ e
P = (np.eye(M) - K @ H) @ P_pre

r = 1/k * ((k-1) * r + Z[:, k] - H @ X_est)
q = 1/k * ((k-1) * q + X[:, k] - F @ X[:, k-1])
R = 1/k * ((k-1) * R + Z[:, k] @ Z[:, k].T - H @ P @ H.T)
Q = 1/k * ((k-1) * Q + K @ e @ e.T @ K.T + P - F @ P @ F.T)

return X, e, P

# 示例参数
F = np.array([[0.0673, 0.1553], [1, 0]])
G = np.array([[1, -0.575], [0, 0]])
H = np.array([[1, 0]])
Q0 = np.diag([0.1, 0.1])
R0 = 0.5
X0 = np.array([0, 0])
Z = np.random.randn(2, 100) # 示例观测数据
P0 = np.eye(2) * 10000
b = 0
s = np.eye(2)

X, e, P = sage_husa_kf(F, G, H, Q0, R0, X0, Z, P0, b, s)