Matrix x_hat; // State estimate

Matrix P; // Error covariance

Matrix K; // Kalman gain

Matrix innovation; // y - C*x_hat (measurement residual)

Matrix A_, B_, C_, D_; // System matrices

Matrix Q_; // Process noise covariance

Matrix R_; // Measurement noise covariance

Matrix x_hat_; // Current state estimate

Matrix P_; // Current error covariance

size_t n_, m_, p_; // Dimensions: states, inputs, outputs

bool initialized_;

/**

KalmanFilter(const Matrix& A, const Matrix& B, const Matrix& C,

const Matrix& Q, const Matrix& R)

: A_(A), B_(B), C_(C), Q_(Q), R_(R), initialized_(false)

{

n_ = A.rows;

m_ = B.cols;

p_ = C.rows;

// Initialize with zero state and large covariance

x_hat_ = Matrix::zeros(n_, 1);

P_ = Matrix::eye(n_) * 1000.0; // Large initial uncertainty

// D matrix defaults to zero

D_ = Matrix::zeros(p_, m_);

}

/**

KalmanFilter(const StateSpace& sys, const Matrix& Q, const Matrix& R)

: KalmanFilter(sys.A, sys.B, sys.C, Q, R)

{

D_ = sys.D;

}

/**

void setInitialState(const Matrix& x0, const Matrix& P0) {

x_hat_ = x0;

P_ = P0;

initialized_ = true;

}

/**

void setInitialState(const Matrix& x0) {

x_hat_ = x0;

initialized_ = true;

}

/**

KalmanEstimate predict(const Matrix& u) {

// Predict state

x_hat_ = A_ * x_hat_ + B_ * u;

// Predict covariance

P_ = A_ * P_ * A_.T() + Q_;

// Symmetrize P for numerical stability

for (size_t i = 0; i < n_; ++i) {

for (size_t j = i + 1; j < n_; ++j) {

double avg = (P_(i, j) + P_(j, i)) / 2.0;

P_(i, j) = P_(j, i) = avg;

}

}

KalmanEstimate est;

est.x_hat = x_hat_;

est.P = P_;

est.K = Matrix::zeros(n_, p_);

est.innovation = Matrix::zeros(p_, 1);

return est;

}

/**

KalmanEstimate correct(const Matrix& y) {

// Innovation (measurement residual)

Matrix y_pred = C_ * x_hat_;

Matrix innovation = y - y_pred;

// Innovation covariance

Matrix S = C_ * P_ * C_.T() + R_;

// Kalman gain

Matrix K = P_ * C_.T() * S.inv();

// Update state estimate

x_hat_ = x_hat_ + K * innovation;

// Update covariance (Joseph form for numerical stability)

Matrix I = Matrix::eye(n_);

Matrix IKC = I - K * C_;

P_ = IKC * P_ * IKC.T() + K * R_ * K.T();

// Symmetrize

for (size_t i = 0; i < n_; ++i) {

for (size_t j = i + 1; j < n_; ++j) {

double avg = (P_(i, j) + P_(j, i)) / 2.0;

P_(i, j) = P_(j, i) = avg;

}

}

KalmanEstimate est;

est.x_hat = x_hat_;

est.P = P_;

est.K = K;

est.innovation = innovation;

return est;

}

/**

KalmanEstimate update(const Matrix& y, const Matrix& u) {

predict(u);

return correct(y);

}

/**

KalmanEstimate update(const Matrix& y) {

Matrix u = Matrix::zeros(m_, 1);

return update(y, u);

}

/**

Matrix getState() const { return x_hat_; }

/**

Matrix getCovariance() const { return P_; }

/**

Matrix steadyStateGain() const {

// Solve DARE using iteration

Matrix P = Matrix::eye(n_);

Matrix P_prev = Matrix::zeros(n_, n_);

for (int iter = 0; iter < 1000; ++iter) {

Matrix CPCT_R = C_ * P * C_.T() + R_;

Matrix CPCT_R_inv = CPCT_R.inv();

Matrix K_temp = P * C_.T() * CPCT_R_inv;

P_prev = P;

P = A_ * P * A_.T() - A_ * K_temp * C_ * P * A_.T() + Q_;

// Check convergence

double diff = 0;

for (size_t i = 0; i < n_; ++i) {

for (size_t j = 0; j < n_; ++j) {

diff += std::abs(P(i, j) - P_prev(i, j));

}

}

if (diff < 1e-10) break;

}

// Compute steady-state gain

Matrix S = C_ * P * C_.T() + R_;

return P * C_.T() * S.inv();

}

/**

void reset() {

x_hat_ = Matrix::zeros(n_, 1);

P_ = Matrix::eye(n_) * 1000.0;

initialized_ = false;

}

Matrix A_, B_, C_;

Matrix K_ss_; // Steady-state Kalman gain

Matrix x_hat_;

size_t n_, m_, p_;

/**

SteadyStateKalmanFilter(const Matrix& A, const Matrix& B, const Matrix& C,

const Matrix& K_ss)

: A_(A), B_(B), C_(C), K_ss_(K_ss)

{

n_ = A.rows;

m_ = B.cols;

p_ = C.rows;

x_hat_ = Matrix::zeros(n_, 1);

}

/**

SteadyStateKalmanFilter(const Matrix& A, const Matrix& B, const Matrix& C,

const Matrix& Q, const Matrix& R)

: A_(A), B_(B), C_(C)

{

n_ = A.rows;

m_ = B.cols;

p_ = C.rows;

x_hat_ = Matrix::zeros(n_, 1);

// Compute steady-state gain

KalmanFilter kf(A, B, C, Q, R);

K_ss_ = kf.steadyStateGain();

}

/**

Matrix update(const Matrix& y, const Matrix& u) {

// Predict

Matrix x_pred = A_ * x_hat_ + B_ * u;

// Correct

Matrix y_pred = C_ * x_pred;

x_hat_ = x_pred + K_ss_ * (y - y_pred);

return x_hat_;

}

Matrix update(const Matrix& y) {

Matrix u = Matrix::zeros(m_, 1);

return update(y, u);

}

Matrix getState() const { return x_hat_; }

Matrix getGain() const { return K_ss_; }

void setInitialState(const Matrix& x0) { x_hat_ = x0; }

using StateFunc = std::function<Matrix(const Matrix&, const Matrix&)>;

using MeasFunc = std::function<Matrix(const Matrix&)>;

using JacobianFunc = std::function<Matrix(const Matrix&, const Matrix&)>;

using MeasJacobianFunc = std::function<Matrix(const Matrix&)>;

StateFunc f_; // State transition function

MeasFunc h_; // Measurement function

JacobianFunc F_; // State Jacobian

MeasJacobianFunc H_; // Measurement Jacobian

Matrix Q_, R_; // Noise covariances

Matrix x_hat_, P_; // State estimate and covariance

size_t n_, m_, p_;

double delta_; // For numerical Jacobian

/**

ExtendedKalmanFilter(StateFunc f, MeasFunc h,

JacobianFunc F, MeasJacobianFunc H,

const Matrix& Q, const Matrix& R,

size_t n, size_t m, size_t p)

: f_(f), h_(h), F_(F), H_(H), Q_(Q), R_(R),

n_(n), m_(m), p_(p), delta_(1e-6)

{

x_hat_ = Matrix::zeros(n_, 1);

P_ = Matrix::eye(n_) * 1000.0;

}

/**

ExtendedKalmanFilter(StateFunc f, MeasFunc h,

const Matrix& Q, const Matrix& R,

size_t n, size_t m, size_t p)

: f_(f), h_(h), Q_(Q), R_(R),

n_(n), m_(m), p_(p), delta_(1e-6)

{

x_hat_ = Matrix::zeros(n_, 1);

P_ = Matrix::eye(n_) * 1000.0;

// Define numerical Jacobians

F_ = [this](const Matrix& x, const Matrix& u) -> Matrix {

return numericalJacobian_f(x, u);

};

H_ = [this](const Matrix& x) -> Matrix {

return numericalJacobian_h(x);

};

}

/**

Matrix numericalJacobian_f(const Matrix& x, const Matrix& u) {

Matrix J(n_, n_);

Matrix f0 = f_(x, u);

for (size_t j = 0; j < n_; ++j) {

Matrix x_pert = x;

x_pert(j, 0) += delta_;

Matrix f_pert = f_(x_pert, u);

for (size_t i = 0; i < n_; ++i) {

J(i, j) = (f_pert(i, 0) - f0(i, 0)) / delta_;

}

}

return J;

}

/**

Matrix numericalJacobian_h(const Matrix& x) {

Matrix J(p_, n_);

Matrix h0 = h_(x);

for (size_t j = 0; j < n_; ++j) {

Matrix x_pert = x;

x_pert(j, 0) += delta_;

Matrix h_pert = h_(x_pert);

for (size_t i = 0; i < p_; ++i) {

J(i, j) = (h_pert(i, 0) - h0(i, 0)) / delta_;

}

}

return J;

}

/**

KalmanEstimate predict(const Matrix& u) {

// Compute Jacobian at current estimate

Matrix F = F_(x_hat_, u);

// Predict state using nonlinear model

x_hat_ = f_(x_hat_, u);

// Predict covariance using linearized model

P_ = F * P_ * F.T() + Q_;

// Symmetrize

for (size_t i = 0; i < n_; ++i) {

for (size_t j = i + 1; j < n_; ++j) {

double avg = (P_(i, j) + P_(j, i)) / 2.0;

P_(i, j) = P_(j, i) = avg;

}

}

KalmanEstimate est;

est.x_hat = x_hat_;

est.P = P_;

return est;

}

/**

KalmanEstimate correct(const Matrix& y) {

// Compute Jacobian at current estimate

Matrix H = H_(x_hat_);

// Predicted measurement using nonlinear model

Matrix y_pred = h_(x_hat_);

Matrix innovation = y - y_pred;

// Innovation covariance

Matrix S = H * P_ * H.T() + R_;

// Kalman gain

Matrix K = P_ * H.T() * S.inv();

// Update state

x_hat_ = x_hat_ + K * innovation;

// Update covariance (Joseph form)

Matrix I = Matrix::eye(n_);

Matrix IKH = I - K * H;

P_ = IKH * P_ * IKH.T() + K * R_ * K.T();

KalmanEstimate est;

est.x_hat = x_hat_;

est.P = P_;

est.K = K;

est.innovation = innovation;

return est;

}

/**

KalmanEstimate update(const Matrix& y, const Matrix& u) {

predict(u);

return correct(y);

}

void setInitialState(const Matrix& x0, const Matrix& P0) {

x_hat_ = x0;

P_ = P0;

}

Matrix getState() const { return x_hat_; }

Matrix getCovariance() const { return P_; }

const StateSpace& sys,

const Matrix& Qc,

const Matrix& R,

double dt

) {

// Simple discretization: Ad = I + A*dt, Bd = B*dt

size_t n = sys.A.rows;

size_t m = sys.B.cols;

Matrix Ad = Matrix::eye(n) + sys.A * dt;

Matrix Bd = sys.B * dt;

// Discretize process noise: Qd ≈ Qc * dt

Matrix Qd = Qc * dt;

return KalmanFilter(Ad, Bd, sys.C, Qd, R);

KalmanFilter& kf,

const std::vector<Matrix>& measurements,

const std::vector<Matrix>& controls

) {

std::vector<Matrix> estimates;

estimates.reserve(measurements.size());

for (size_t k = 0; k < measurements.size(); ++k) {

Matrix u = (k < controls.size()) ? controls[k] : Matrix::zeros(controls[0].rows, 1);

auto est = kf.update(measurements[k], u);

estimates.push_back(est.x_hat);

}

return estimates;

/**

enum class Method {

INNOVATION_CORRELATION, // Innovation-based (Mehra)

COVARIANCE_MATCHING, // Match predicted vs actual covariance

SAGE_HUSA, // Sage-Husa adaptive algorithm

VARIATIONAL_BAYES // Variational Bayesian approach

};

Matrix A_, B_, C_;

Matrix Q_, R_; // Current noise covariances

Matrix Q_init_, R_init_; // Initial values (for bounds)

Matrix x_hat_, P_; // State and covariance

// Adaptation parameters

Method method_;

double alpha_; // Forgetting factor (0 < α < 1)

size_t window_size_; // Window for sample covariance

size_t min_samples_; // Minimum samples before adaptation

// Innovation history for adaptation

std::vector<Matrix> innovation_history_;

Matrix S_sample_; // Sample innovation covariance

// Bounds for Q and R (prevent divergence)

double q_min_, q_max_;

double r_min_, r_max_;

// Statistics

size_t n_, m_, p_;

size_t step_count_;

bool initialized_;

/**

AdaptiveKalmanFilter(

const Matrix& A, const Matrix& B, const Matrix& C,

const Matrix& Q_init, const Matrix& R_init,

Method method = Method::INNOVATION_CORRELATION,

double alpha = 0.95

) : A_(A), B_(B), C_(C),

Q_(Q_init), R_(R_init),

Q_init_(Q_init), R_init_(R_init),

method_(method), alpha_(alpha),

window_size_(20), min_samples_(10),

q_min_(1e-10), q_max_(1e6),

r_min_(1e-10), r_max_(1e6),

step_count_(0), initialized_(false)

{

n_ = A.rows;

m_ = B.cols;

p_ = C.rows;

x_hat_ = Matrix::zeros(n_, 1);

P_ = Matrix::eye(n_);

S_sample_ = Matrix::zeros(p_, p_);

}

/**

void setInitialState(const Matrix& x0, const Matrix& P0) {

x_hat_ = x0;

P_ = P0;

initialized_ = true;

}

/**

void setAdaptationParams(double alpha, size_t window_size, size_t min_samples) {

alpha_ = alpha;

window_size_ = window_size;

min_samples_ = min_samples;

}

/**

void setBounds(double q_min, double q_max, double r_min, double r_max) {

q_min_ = q_min;

q_max_ = q_max;

r_min_ = r_min;

r_max_ = r_max;

}

/**

KalmanEstimate update(const Matrix& y, const Matrix& u) {

step_count_++;

// === PREDICTION STEP ===

Matrix x_pred = A_ * x_hat_ + B_ * u;

Matrix P_pred = A_ * P_ * A_.T() + Q_;

// === CORRECTION STEP ===

Matrix CT = C_.T();

Matrix S = C_ * P_pred * CT + R_; // Innovation covariance

Matrix S_inv = S.inv();

// Kalman gain

Matrix K = P_pred * CT * S_inv;

// Innovation (measurement residual)

Matrix innovation = y - C_ * x_pred;

// State update

x_hat_ = x_pred + K * innovation;

// Covariance update (Joseph form for numerical stability)

Matrix I = Matrix::eye(n_);

Matrix IKC = I - K * C_;

P_ = IKC * P_pred * IKC.T() + K * R_ * K.T();

// === ADAPTATION STEP ===

adaptNoiseCovariances(innovation, K, P_pred);

// Return result

KalmanEstimate est;

est.x_hat = x_hat_;

est.P = P_;

est.K = K;

est.innovation = innovation;

return est;

}

/**

Matrix getQ() const { return Q_; }

/**

Matrix getR() const { return R_; }

/**

Matrix getState() const { return x_hat_; }

/**

Matrix getCovariance() const { return P_; }

/**

size_t getStepCount() const { return step_count_; }

/**

void reset() {

x_hat_ = Matrix::zeros(n_, 1);

P_ = Matrix::eye(n_);

Q_ = Q_init_;

R_ = R_init_;

S_sample_ = Matrix::zeros(p_, p_);

innovation_history_.clear();

step_count_ = 0;

initialized_ = false;

}

/**

void adaptNoiseCovariances(const Matrix& innovation, const Matrix& K,

const Matrix& P_pred) {

// Store innovation for windowed estimation

innovation_history_.push_back(innovation);

if (innovation_history_.size() > window_size_) {

innovation_history_.erase(innovation_history_.begin());

}

// Don't adapt until we have enough samples

if (step_count_ < min_samples_) {

return;

}

switch (method_) {

case Method::INNOVATION_CORRELATION:

adaptInnovationCorrelation(innovation, K, P_pred);

break;

case Method::COVARIANCE_MATCHING:

adaptCovarianceMatching(innovation, K, P_pred);

break;

case Method::SAGE_HUSA:

adaptSageHusa(innovation, K, P_pred);

break;

case Method::VARIATIONAL_BAYES:

adaptVariationalBayes(innovation, K, P_pred);

break;

}

}

/**

void adaptInnovationCorrelation(const Matrix& innovation, const Matrix& K,

const Matrix& P_pred) {

// Update sample innovation covariance with exponential forgetting

Matrix vvT = innovation * innovation.T();

if (step_count_ == min_samples_) {

S_sample_ = vvT;

} else {

for (size_t i = 0; i < p_; ++i) {

for (size_t j = 0; j < p_; ++j) {

S_sample_(i, j) = alpha_ * S_sample_(i, j) +

(1.0 - alpha_) * vvT(i, j);

}

}

}

// Theoretical innovation covariance: S = C*P_pred*C' + R

Matrix CT = C_.T();

Matrix CPCT = C_ * P_pred * CT;

// Adapt R: R_new = S_sample - C*P_pred*C'

Matrix R_new = S_sample_;

for (size_t i = 0; i < p_; ++i) {

for (size_t j = 0; j < p_; ++j) {

R_new(i, j) -= CPCT(i, j);

}

}

// Ensure R is positive definite (diagonal dominance)

for (size_t i = 0; i < p_; ++i) {

R_new(i, i) = clamp(R_new(i, i), r_min_, r_max_);

for (size_t j = 0; j < p_; ++j) {

if (i != j) {

// Limit off-diagonal to maintain positive definiteness

double max_off = 0.9 * std::sqrt(R_new(i, i) * R_new(j, j));

R_new(i, j) = clamp(R_new(i, j), -max_off, max_off);

}

}

}

// Adapt Q: Q_new = K * S_sample * K'

Matrix KT = K.T();

Matrix Q_new = K * S_sample_ * KT;

// Ensure Q is positive definite

for (size_t i = 0; i < n_; ++i) {

Q_new(i, i) = clamp(Q_new(i, i), q_min_, q_max_);

for (size_t j = 0; j < n_; ++j) {

if (i != j) {

double max_off = 0.9 * std::sqrt(Q_new(i, i) * Q_new(j, j));

Q_new(i, j) = clamp(Q_new(i, j), -max_off, max_off);

}

}

}

// Smooth update (avoid abrupt changes)

double beta = 0.1; // Learning rate

for (size_t i = 0; i < p_; ++i) {

for (size_t j = 0; j < p_; ++j) {

R_(i, j) = (1 - beta) * R_(i, j) + beta * R_new(i, j);

}

}

for (size_t i = 0; i < n_; ++i) {

for (size_t j = 0; j < n_; ++j) {

Q_(i, j) = (1 - beta) * Q_(i, j) + beta * Q_new(i, j);

}

}

}

/**

void adaptCovarianceMatching(const Matrix& innovation, const Matrix& K,

const Matrix& P_pred) {

// Compute sample covariance from window

if (innovation_history_.size() < min_samples_) return;

// Sample mean

Matrix mean = Matrix::zeros(p_, 1);

for (const auto& v : innovation_history_) {

for (size_t i = 0; i < p_; ++i) {

mean(i, 0) += v(i, 0);

}

}

for (size_t i = 0; i < p_; ++i) {

mean(i, 0) /= innovation_history_.size();

}

// Sample covariance

Matrix S_sample = Matrix::zeros(p_, p_);

for (const auto& v : innovation_history_) {

Matrix centered = v - mean;

for (size_t i = 0; i < p_; ++i) {

for (size_t j = 0; j < p_; ++j) {

S_sample(i, j) += centered(i, 0) * centered(j, 0);

}

}

}

for (size_t i = 0; i < p_; ++i) {

for (size_t j = 0; j < p_; ++j) {

S_sample(i, j) /= (innovation_history_.size() - 1);

}

}

// Scale factor: ratio of sample to predicted covariance

Matrix CT = C_.T();

Matrix S_pred = C_ * P_pred * CT + R_;

// For simplicity, use trace ratio

double trace_sample = 0, trace_pred = 0;

for (size_t i = 0; i < p_; ++i) {

trace_sample += S_sample(i, i);

trace_pred += S_pred(i, i);

}