PydaptiveFiltering

import numpy as np
import matplotlib.pyplot as plt
import pydaptivefiltering as pdf

rng = np.random.default_rng(0)
w_true = np.array([0.5+0.2j, -0.4+0.5j,
 0.1-0.1j, 0.2+0j,
 -0.1+0.1j, 0.0+0.05j])
M, N = len(w_true) - 1, 300

x = (rng.standard_normal(N) + 1j*rng.standard_normal(N)) / np.sqrt(2)
noise = 0.05 * (rng.standard_normal(N) + 1j*rng.standard_normal(N)) / np.sqrt(2)
x_pad = np.r_[np.zeros(M, complex), x]
d = np.array([np.vdot(w_true, x_pad[k:k+M+1][::-1]) for k in range(N)]) + noise

res = pdf.RLS(filter_order=M, delta=1.0, forgetting_factor=0.995).optimize(x, d)

offset, win = 10, 20
mse = np.abs(np.asarray(res.errors).ravel())**2
mse_s = np.convolve(mse, np.ones(win)/win, mode="same")

W = np.asarray(res.coefficients)
if W.ndim == 2 and W.shape[0] == M + 1: W = W.T
Wm, wt = np.abs(W), np.abs(w_true)

fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 5))
t = np.arange(len(mse))[offset:]
ax1.semilogy(t, mse[offset:], alpha=0.25)
ax1.semilogy(t, mse_s[offset:], lw=2)
ax1.set_title(f"RLS convergence (from {offset})")
ax1.set_xlabel("k"); ax1.set_ylabel("MSE (log)")
ax1.grid(True, which="both", alpha=0.25)

tw = np.arange(Wm.shape[0])[offset:]
for i in range(Wm.shape[1]):
    ax2.plot(tw, Wm[offset:, i], label=f"|w{i}|")
    ax2.axhline(wt[i], ls="--", alpha=0.6)
ax2.set_title("Coefficient magnitude vs. true values")
ax2.set_xlabel("k"); ax2.set_ylabel("|w|")
ax2.grid(True, alpha=0.25); ax2.legend(fontsize="small", ncol=2)
plt.tight_layout(); plt.show()

import numpy as np
import matplotlib.pyplot as plt
import pydaptivefiltering as pdf

rng = np.random.default_rng(7)

N = 400
dt = 1.0

x_true = np.zeros((N, 2), dtype=float)

a = np.zeros(N, dtype=float)
a[60:120]  =  0.08   # acelera
a[180:230] = -0.12   # freia
a[300:340] =  0.05   # acelera de leve

x_true[0] = [0.0, 1.0]
for k in range(1, N):
    pos_prev, vel_prev = x_true[k-1]
    vel = vel_prev + a[k] * dt
    pos = pos_prev + vel_prev * dt + 0.5 * a[k] * dt**2
    x_true[k] = [pos, vel]

# Medição: posição com ruído
sigma_meas = 0.8
y_meas = x_true[:, 0] + sigma_meas * rng.standard_normal(N)

A = np.array([[1.0, dt],
              [0.0, 1.0]], dtype=float)

C_T = np.array([[1.0, 0.0]], dtype=float)   # shape (p=1, n=2)

sigma_a = 0.15
Q = (sigma_a**2) * np.array([
    [dt**4/4, dt**3/2],
    [dt**3/2, dt**2]
], dtype=float)

Rn  = Q
Rn1 = np.array([[sigma_meas**2]], dtype=float)

x_init  = np.array([y_meas[0], 0.0], dtype=float)
Re_init = np.eye(2, dtype=float) * 50.0

kf = pdf.Kalman(A, C_T, Rn, Rn1, x_init, Re_init)

res = kf.optimize(y_meas, verbose=True)

x_hat = res.outputs
innov = res.errors
x_hat = np.asarray(x_hat)
innov = np.asarray(innov).ravel()

t = np.arange(N)

fig1, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6), sharex=True)

ax1.plot(t, x_true[:, 0], label="True position")
ax1.plot(t, y_meas, ".", markersize=3, alpha=0.5, label="Measured position")
ax1.plot(t, x_hat[:, 0], label="Kalman estimate (position)")
ax1.set_ylabel("Position")
ax1.legend(loc="best")

ax2.plot(t, x_true[:, 1], label="True velocity")
ax2.plot(t, x_hat[:, 1], label="Kalman estimate (velocity)")
ax2.set_xlabel("Time index")
ax2.set_ylabel("Velocity")
ax2.legend(loc="best")

fig1.suptitle("Kalman filter (CV model) with maneuvers")
fig1.tight_layout()

plt.show()