Problem: Kalman Filter Core Functions Implementation

The problem involves building enhanced Kalman filter components in Python: state prediction with multi-dimensional/batch support and nonlinear dynamics, covariance prediction ensuring symmetry/positive definiteness, Kalman gain calculation with numerical stability, and state/covariance update handling batches—all with input validation and type compatibility for robust filter operations.

Background: ### Industry Background Knowledge for Kalman Filter Implementation Reproduction
---

#### 1. Kalman Filter Industry Applications Overview
The provided functions implement core components of an enhanced Kalman Filter (KF), a statistical estimation algorithm widely adopted in:
- **Aerospace**: Satellite orbit prediction, missile guidance, and aircraft state estimation (where nonlinear drag/Coriolis effects are critical for long-range tracking)
- **Automotive**: ADAS (Advanced Driver-Assistance Systems) for object tracking, autonomous vehicle localization, and sensor fusion
- **Robotics**: Robot arm state estimation, drone navigation, and SLAM (Simultaneous Localization and Mapping)
- **Surveillance**: Multi-target tracking in video analytics
- **Finance**: Time series forecasting for asset price modeling

These functions extend the standard KF with industry-grade robustness features to address real-world challenges like numerical instability, multi-dimensional motion, batch processing for multiple targets, and compatibility with diverse input data formats.

#### 2. State Prediction (`predict_state`)
**Mathematical Basis**:
The core linear constant-velocity model uses the state transition matrix:
\[
F = \begin{bmatrix}1 & dt \\0 & 1\end{bmatrix}
\]
For multi-dimensional systems (2D/3D motion), this matrix is block-diagonally extended using Kronecker product: \(F_{nD} = I_n \otimes F\), where \(I_n\) is the identity matrix for spatial dimensions.

**Industry Enhancements**:
- **Nonlinear Terms**: Quadratic drag (\(F_{drag} = -c \cdot v \cdot |v| \cdot dt\)) compensates for air/water resistance in aerospace/underwater tracking. The Coriolis effect (\(F_{coriolis} = 2\Omega \times v\)) is critical for long-range Earth-based tracking (using Earth's rotation rate \(\Omega = 7.292e-5\) rad/s).
- **Batch Processing**: Supports 2D array inputs where each row represents a target state, enabling real-time multi-target tracking.
- **Numerical Stability**: Value clipping (\(\pm1e18\)) prevents floating-point overflow in embedded systems or long-running simulations.
- **Type Compatibility**: Converts between lists and numpy arrays to integrate with legacy codebases (common in automotive/aerospace systems).

#### 3. Covariance Prediction (`predict_covariance`)
**Mathematical Formulation**:
The standard KF covariance update formula:
\[
P_{k+1} = F_k P_k F_k^T + Q_k
\]
where \(P\) is the state covariance matrix, \(Q\) is the process noise covariance matrix.

**Industry Stability Considerations**:
- **Symmetry Enforcement**: Covariance matrices must be symmetric. The code enforces this by averaging \(res\) and \(res^T\) to compensate for floating-point precision errors.
- **Positive Definiteness**: Adds machine epsilon (\(\epsilon = np.finfo(np.float64).eps\)) to the diagonal to prevent singular matrices, which would break subsequent KF updates in real-time systems.
- **Optimized Computation**: Uses BLAS/LAPACK-accelerated matrix operations (via numpy) for low-latency performance in time-critical applications (e.g., autonomous vehicle control loops).

#### 4. Kalman Gain Calculation (`calculate_kalman_gain`)
**Mathematical Formula**:
\[
K = P_{prior} H^T (H P_{prior} H^T + R)^{-1}
\]
where \(H\) is the measurement matrix, \(R\) is the measurement noise covariance.

**Industry Robustness Features**:
- **Ill-Conditioned Matrix Handling**: Uses Moore-Penrose pseudoinverse (\(np.linalg.pinv\)) instead of regular inverse to handle singular or rank-deficient innovation covariance matrices (common in redundant sensor setups, e.g., multiple cameras in ADAS).
- **Tikhonov Regularization**: Adds scaled epsilon to the innovation covariance matrix to stabilize inversion in noisy measurement environments.
- **Flexible Noise Input**: Supports scalar, 1D, and 2D \(R\) values to model both uncorrelated (diagonal) and correlated measurement noise, a requirement in sensor fusion systems with heterogeneous sensors (radar + camera + LiDAR).

#### 5. State & Covariance Update (`update_state`)
**Mathematical Basis**:
The KF update equations:
\[
x_{new} = x_{prior} + K(z - H x_{prior})
\]
\[
P_{new} = (I - KH) P_{prior}
\]
where \(z\) is the measurement vector.

**Industry Multi-Target Support**:
- **Batch Processing**: Vectorized operations handle multiple target states simultaneously, reducing computational overhead in surveillance and multi-robot systems.
- **Low-Rank Measurement Matrices**: Uses SVD (Singular Value Decomposition) to stabilize computations for low-rank \(H\) matrices (e.g., when sensors provide redundant measurements).
- **Rigorous Validation**: Checks for covariance matrix symmetry and positive semi-definiteness to ensure filter robustness in edge cases (e.g., sensor failure leading to invalid noise values).

#### 6. Cross-Cutting Industry Best Practices
- **Input Validation**: Comprehensive checks for invalid dimensions, non-positive time steps, and invalid matrix properties prevent runtime crashes in safety-critical systems.
- **Type Consistency**: Preserves input types (lists/numpy arrays) to integrate with existing codebases without refactoring.
- **Error Handling**: Explicit error messages simplify debugging in production environments, reducing downtime in mission-critical applications.
- **Complex Number Support**: Enables phase-space/frequency-domain KF applications in signal processing (e.g., radar signal analysis).

Input/Output: The problem receives four sets of inputs: 1) For state prediction: a 1D/2D even-length array-like state (combining positions and velocities) and a positive float time step (dt); 2) For covariance prediction: a symmetric 2x2 list/array (prior covariance P), a 2x2 list/array (state transition matrix F), and a symmetric 2x2 list/array (process noise covariance Q); 3) For Kalman gain calculation: a square matrix (prior covariance P_prior), a 2D matrix (observation matrix H), and a scalar/1D/2D square matrix (measurement noise covariance R); 4) For state update: a 1D/2D array-like (prior state x_prior), a 2D/3D array-like (prior covariance P_prior), a list/array (Kalman gain K), a scalar/list/array (observation z), and a 2D matrix (observation matrix H). The problem generates four outputs: 1) A predicted state with the same type as the input state; 2) A symmetric, positive-definite predicted covariance as a list of lists; 3) A Kalman gain vector as a list of floats (first column of the gain matrix); 4) A new state (list) and new covariance (list of lists) supporting batch processing. Each input set is processed by its corresponding function—validating inputs, performing computations (nonlinear state transition, covariance matrix operations, stabilized gain calculation, state/covariance update via innovation)—to produce the respective output.

Required dependencies:
import numpy as np
import math
import warnings
from typing import Union, List, Tuple

Sub-steps:
step_number: 1
step_description_prompt: Please write a Python function named predict_state. It predicts next state using a constant-velocity model with enhancements: multi-dimensional support, batch processing, nonlinear drag/Coriolis terms, numerical clipping, input type compatibility. Args: state (1D/2D array-like, even length), dt (positive float). Return same type as input; include validations for state dims, dt positivity, even state length.
function_header: def predict_state(state, dt):
test_cases: import math
for state, dt, expected in [
    ([0, 10], 1.0, [10.0, 9.0]),   # Updated: Enhanced formula with nonlinear dynamics
    ([100, -5], 2.0, [90.0, -4.5]), # Updated: Enhanced formula with nonlinear dynamics
    ([50, 0], 10.0, [50.0, 0.0])    # 速度为0，位移保持不变
]:
    res = predict_state(state, dt)
    assert all(math.isclose(r, e) for r, e in zip(res, expected))

step_number: 2
step_description_prompt: Please write a Python function named predict_covariance. It computes next covariance matrix in Kalman filter: Pₖ₊₁=FₖPₖFₖᵀ+Qₖ. Validate inputs are 2x2 lists/arrays, symmetric P/Q. Use numpy for stability, enforce result symmetry and positive definiteness, return list of lists.
function_header: def predict_covariance(P, F, Q):
test_cases: import math
F_id = [[1, 1], [0, 1]] # 典型状态转移矩阵
Q_zero = [[0, 0], [0, 0]]
for P, F, Q, exp_p00 in [
    ([[1, 0], [0, 1]], F_id, Q_zero, 2.0), # F*I*F.T -> [[2, 1], [1, 1]]
    ([[0, 0], [0, 0]], F_id, [[1, 0], [0, 1]], 1.0), # P为0时结果等于Q
    ([[1, 0], [0, 1]], [[1, 0], [0, 1]], [[1, 0], [0, 1]], 2.0) # I*I*I + I = 2I
]:
    res = predict_covariance(P, F, Q)
    assert math.isclose(res[0][0], exp_p00)

step_number: 3
step_description_prompt: Please write a Python function named calculate_kalman_gain. It computes Kalman Gain from P_prior, H, R. Steps: convert inputs to float64, validate dimensions/types, compute innovation covariance (HPH^T + R; handle scalar/1D R as diagonal), stabilize inversion with epsilon, use pseudoinverse for gain, return first column as float list. Handle errors for invalid inputs/ranks.
function_header: def calculate_kalman_gain(P_prior, H, R):
test_cases: import math
H_pos = [[1, 0]] # 仅观测位置 - must be 2D matrix
for P, H, R, exp_K in [
    ([[1, 0], [0, 1]], H_pos, 1.0, [0.5, 0.0]),  # P=1, R=1 -> K=1/(1+1)=0.5
    ([[10, 5], [5, 10]], H_pos, 0.0, [1.0, 0.5]), # R=0, 信任观测, K0=10/10=1
    ([[0, 0], [0, 0]], H_pos, 1.0, [0.0, 0.0])   # 无先验误差
]:
    res = calculate_kalman_gain(P, H, R)
    assert all(math.isclose(r, e) for r, e in zip(res, exp_K))

step_number: 4
step_description_prompt: Please write a Python function named update_state. It performs Kalman filter update: takes x_prior (state prior), P_prior (covariance prior), K (Kalman gain), z (observation), H (observation matrix). Converts inputs to numpy float64, validates dimensions, computes innovation, updates state and covariance using I-KH@P, handles batches, returns new state and covariance as lists.
function_header: def update_state(x_prior, P_prior, K, z, H):
test_cases: import math
H_pos = [[1, 0]]  # Must be 2D matrix
test_data_update = [
    ([10.0, 2.0], [[1.0, 0.0], [0.0, 1.0]], [0.5, 0.1], 10.0, [10.0, 2.0], [[0.5, 0.0], [-0.1, 1.0]]),
    ([10.0, 2.0], [[1.0, 0.0], [0.0, 1.0]], [1.0, 0.5], 12.0, [12.0, 3.0], [[0.0, 0.0], [-0.5, 1.0]]),
    
    ([0.0, 0.0], [[10.0, 0.0], [0.0, 10.0]], [0.8, 0.4], 10.0, [8.0, 4.0], [[2.0, 0.0], [-4.0, 10.0]])
]

for xp, Pp, K, z, exp_x, exp_P in test_data_update:
    new_x, new_P = update_state(xp, Pp, K, z, H_pos)
    
    assert all(math.isclose(r, e, abs_tol=1e-7) for r, e in zip(new_x, exp_x))
    
    assert all(math.isclose(new_P[i][j], exp_P[i][j], abs_tol=1e-7) 
               for i in range(2) for j in range(2))

General solution:

General tests: