Spaces:
Running on Zero
Running on Zero
File size: 5,060 Bytes
9936912 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 | """Estimation, Kalman filtering, and system identification tools."""
from __future__ import annotations
from typing import Any
import numpy as np
from controlai_agent.registry import registry
@registry.register(
name="kalman_time_update",
description="Perform Kalman filter time update (state and covariance prediction) for x[k+1] = A x[k] + B u[k] + w[k].",
parameters_schema={
"type": "object",
"properties": {
"A": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}},
"B": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}},
"x_plus": {"type": "array", "items": {"type": "number"}, "description": "Prior state estimate x_hat+[k-1]"},
"u": {"type": "array", "items": {"type": "number"}, "description": "Control input u[k-1]"},
"P_plus": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}, "description": "Prior error covariance P+[k-1]"},
"Q": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}, "description": "Process noise covariance Q"},
},
"required": ["A", "x_plus", "P_plus", "Q"],
},
)
def kalman_time_update(
A: list[list[float]],
P_plus: list[list[float]],
Q: list[list[float]],
x_plus: list[float],
B: list[list[float]] | None = None,
u: list[float] | None = None,
) -> dict[str, Any]:
A_mat = np.array(A, dtype=float)
P_mat = np.array(P_plus, dtype=float)
Q_mat = np.array(Q, dtype=float)
x_vec = np.array(x_plus, dtype=float)
x_minus = A_mat @ x_vec
if B is not None and u is not None:
B_mat = np.array(B, dtype=float)
u_vec = np.array(u, dtype=float)
x_minus += B_mat @ u_vec
P_minus = A_mat @ P_mat @ A_mat.T + Q_mat
return {
"x_minus_predicted": x_minus.tolist(),
"P_minus_predicted": P_minus.tolist(),
"is_symmetric": bool(np.max(np.abs(P_minus - P_minus.T)) < 1e-10),
}
@registry.register(
name="kalman_measurement_update",
description="Perform Kalman filter measurement update using Joseph-form covariance update to ensure numerical stability and symmetry.",
parameters_schema={
"type": "object",
"properties": {
"x_minus": {"type": "array", "items": {"type": "number"}, "description": "Prior state estimate x_hat-[k]"},
"P_minus": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}, "description": "Prior error covariance P-[k]"},
"H": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}, "description": "Measurement matrix H"},
"R": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}, "description": "Measurement noise covariance R"},
"z": {"type": "array", "items": {"type": "number"}, "description": "Sensor measurement vector z[k]"},
},
"required": ["x_minus", "P_minus", "H", "R", "z"],
},
)
def kalman_measurement_update(
x_minus: list[float],
P_minus: list[list[float]],
H: list[list[float]],
R: list[list[float]],
z: list[float],
) -> dict[str, Any]:
x_m = np.array(x_minus, dtype=float)
P_m = np.array(P_minus, dtype=float)
H_mat = np.array(H, dtype=float)
R_mat = np.array(R, dtype=float)
z_vec = np.array(z, dtype=float)
innovation = z_vec - H_mat @ x_m
S = H_mat @ P_m @ H_mat.T + R_mat
K = P_m @ H_mat.T @ np.linalg.inv(S)
x_plus = x_m + K @ innovation
# Joseph stabilized form
I = np.eye(len(x_m))
P_plus = (I - K @ H_mat) @ P_m @ (I - K @ H_mat).T + K @ R_mat @ K.T
return {
"innovation": innovation.tolist(),
"innovation_covariance_S": S.tolist(),
"kalman_gain_K": K.tolist(),
"x_plus_updated": x_plus.tolist(),
"P_plus_updated": P_plus.tolist(),
}
@registry.register(
name="least_squares_arx",
description="Batch ordinary least squares parameter estimation for ARX / linear regression model Y = Phi * theta + e.",
parameters_schema={
"type": "object",
"properties": {
"Phi": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}, "description": "Regressor matrix (N x p)"},
"Y": {"type": "array", "items": {"type": "array", "items": {"type": "number"}}, "description": "Measurement output vector (N x 1)"},
},
"required": ["Phi", "Y"],
},
)
def least_squares_arx(Phi: list[list[float]], Y: list[list[float]]) -> dict[str, Any]:
Phi_mat = np.array(Phi, dtype=float)
Y_mat = np.array(Y, dtype=float)
Gram = Phi_mat.T @ Phi_mat
theta = np.linalg.solve(Gram, Phi_mat.T @ Y_mat)
residual = Y_mat - Phi_mat @ theta
res_norm = float(np.linalg.norm(residual))
cond = float(np.linalg.cond(Gram))
return {
"theta_estimated": theta.tolist(),
"residual_2norm": res_norm,
"gram_matrix_condition_number": cond,
"is_identifiable": cond < 1e12,
}
|