I am simulating a system in which I do not have very accurate information about the measurement and process noises (R and Q). However, although my linear Kalman filter works, it seems that there is some error, since at the initial moments the filter decreases and stabilizes. Since my estimated P matrix has a magnitude of 1e-5, I thought it would be better to redefine it… but I don’t know how to do it. I would like to know if this behavior is expected and if my code is correct.
trace curve
xpred = np.zeros((num_medicoes, nestados))
x_estimado = np.zeros((num_medicoes, nestados))
Ppred = np.zeros((num_medicoes, nestados, nestados))
P_estimado = np.zeros((num_medicoes, nestados, nestados))
K = np.zeros((num_medicoes, nestados, nsaidas)) # Ganho de Kalman
I = np.eye(nestados)
erro_covariancia = np.zeros(num_medicoes)
# Variáveis para monitoramento e reset
traco = np.zeros(num_medicoes)
autovalores_minimos = np.zeros(num_medicoes)
reset_points = [] # Armazena índices onde P foi resetado
min_eig_threshold = 1e-6# Limiar para autovalor mínimo
#cond_threshold = 1e8 # Limiar para número de condição
inflation_factor = 10.0 # Fator de inflação para P após reset
min_reset_interval = 5
fading_threshold = 1e-2 # Antecipado para atuar antes
fading_factor = 1.5 # Mais agressivo
K_valor = np.zeros(num_medicoes)
# Inicialização
x_estimado[0] = x0.reshape(-1)
P_estimado[0] = p0
# Processamento recursivo - Filtro de Kalman
for i in range(num_medicoes):
if i == 0:
# Passo de predição inicial
xpred[i] = A @ x0
Ppred[i] = A @ p0 @ A.T + Q
else:
# Passo de predição
xpred[i] = A @ x_estimado[i-1]
Ppred[i] = A @ P_estimado[i-1] @ A.T + Q
# Cálculo do ganho de Kalman
S = C @ Ppred[i] @ C.T + R
K[i] = Ppred[i] @ C.T @ np.linalg.inv(S)
K_valor[i]= K[i]
## erro de covariancia
erro_covariancia[i] = C @ Ppred[i] @ C.T
# Atualização / Correção
y_residual = y[i] - (C @ xpred[i].reshape(-1, 1)).flatten()
x_estimado[i] = xpred[i] + K[i] @ y_residual
P_estimado[i] = (I - K[i] @ C) @ Ppred[i]
# Verificação de estabilidade numérica
#eigvals, eigvecs = np.linalg.eigh(P_estimado[i])
eigvals = np.linalg.eigvalsh(P_estimado[i])
min_eig = np.min(eigvals)
autovalores_minimos[i] = min_eig
#cond_number = np.max(eigvals) / min_eig if min_eig > 0 else np.inf
# Reset adaptativo da matriz de covariância
#if min_eig < min_eig_threshold or cond_number > cond_threshold:
# RESET MODIFICADO - ESTRATÉGIA HÍBRIDA
if (min_eig < min_eig_threshold) and (i - reset_points[-1] > min_reset_interval if reset_points else True):
print(f"[{i}] Reset: min_eig = {min_eig:.2e}")
# Método 1: Inflação proporcional ao traço médio histórico
mean_trace = np.mean(traco[max(0,i-10):i]) if i > 0 else np.trace(p0)
P_estimado[i] = 0.5 * (P_estimado[i] + np.eye(nestados) * mean_trace/nestados)
# Método 2: Reinicialização parcial para p0
alpha = 0.3
P_estimado[i] = alpha*p0 + (1-alpha)*P_estimado[i]
reset_points.append(i)
# FADING MEMORY ANTECIPADO
current_trace = np.trace(P_estimado[i])
if current_trace < fading_threshold:
# Fator adaptativo: quanto menor o traço, maior o ajuste
adaptive_factor = 1 + (fading_threshold - current_trace)/fading_threshold
P_estimado[i] *= adaptive_factor
print(f"[{i}] Fading: traço = {current_trace:.2e} -> {np.trace(P_estimado[i]):.2e}")
# Armazena o traço para análise
traco[i] = np.trace(P_estimado[i])