18389801194

Committed 09 Oct 2025 09:35PM UTC coverage: 71.943% (+0.1%) from 71.826%

Build # 18389801194

Build Type

push

github

Committed by

paulmthompson

Commit Message

add correlation matrix to filtering interface

Run Details

207 of 337 new or added lines in 5 files covered. (61.42%)

867 existing lines in 31 files now uncovered.

49964 of 69449 relevant lines covered (71.94%)

1103.53 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

88.71

/src/StateEstimation/Kalman/KalmanFilter.cpp

#include "KalmanFilter.hpp"


#include <iostream>
#include <stdexcept>

namespace StateEstimation {

KalmanFilter::KalmanFilter(Eigen::MatrixXd const & F, Eigen::MatrixXd const & H,
                           Eigen::MatrixXd const & Q, Eigen::MatrixXd const & R)
    : F_(F),
      H_(H),
      Q_(Q),
      R_(R),
      x_(Eigen::VectorXd::Zero(F.rows())),
      P_(Eigen::MatrixXd::Identity(F.rows(), F.rows())),
      F_inv_(Eigen::MatrixXd::Zero(F.rows(), F.cols())),
      Q_backward_(Q.rows(), Q.cols()) {
    if (F_.rows() == F_.cols()) {
        // Compute inverse if well-conditioned; allow Eigen to throw if singular
        F_inv_ = F_.inverse();
        // Backward process noise: Q_b = F_inv * Q * F_inv^T (approximate mapping)
        Q_backward_ = F_inv_ * Q_ * F_inv_.transpose();
        // Ensure symmetry
        Q_backward_ = 0.5 * (Q_backward_ + Q_backward_.transpose());
    }
}

void KalmanFilter::initialize(FilterState const & initial_state) {
    x_ = initial_state.state_mean;
    P_ = initial_state.state_covariance;
}

FilterState KalmanFilter::predict() {
    x_ = F_ * x_;
    P_ = F_ * P_ * F_.transpose() + Q_;
    return FilterState{.state_mean = x_, .state_covariance = P_};
}

FilterState KalmanFilter::update(FilterState const & predicted_state, Measurement const & measurement) {
    return update(predicted_state, measurement, 1.0);
}

FilterState KalmanFilter::update(FilterState const & predicted_state, Measurement const & measurement, double noise_scale_factor) {
    Eigen::VectorXd const & z = measurement.feature_vector;

    // Use the predicted state passed in
    Eigen::VectorXd const x_pred = predicted_state.state_mean;
    Eigen::MatrixXd const P_pred = predicted_state.state_covariance;

    // Scale the measurement noise matrix R by the noise scale factor
    Eigen::MatrixXd const R_scaled = noise_scale_factor * R_;

    Eigen::VectorXd const y = z - H_ * x_pred;                      // Innovation or residual
    Eigen::MatrixXd S = H_ * P_pred * H_.transpose() + R_scaled;    // Innovation covariance
    Eigen::MatrixXd K = P_pred * H_.transpose() * S.inverse();// Kalman gain

    x_ = x_pred + K * y;
    P_ = (Eigen::MatrixXd::Identity(x_.size(), x_.size()) - K * H_) * P_pred;

    return FilterState{.state_mean = x_, .state_covariance = P_};
}

std::vector<FilterState> KalmanFilter::smooth(std::vector<FilterState> const & forward_states) {
    if (forward_states.empty()) {
        return {};
    }

    std::vector<FilterState> smoothed_states = forward_states;

    // The backward pass (Rauch-Tung-Striebel smoother)
    // Propagate information backward from k+1 to k
    for (int k = static_cast<int>(forward_states.size()) - 2; k >= 0; --k) {
        FilterState const & fwd_k = forward_states[k];
        FilterState const & smoothed_k_plus_1 = smoothed_states[k + 1];

        // Prediction for k+1 based on forward estimate at k
        Eigen::VectorXd x_pred_k_plus_1 = F_ * fwd_k.state_mean;
        Eigen::MatrixXd P_pred_k_plus_1 = F_ * fwd_k.state_covariance * F_.transpose() + Q_;

        // Smoother gain: C_k = P_k * F^T * P_pred^{-1}
        Eigen::MatrixXd Ck = fwd_k.state_covariance * F_.transpose() * P_pred_k_plus_1.inverse();

        // Smoothed estimate at k incorporating information from future (k+1)
        smoothed_states[k].state_mean = fwd_k.state_mean + Ck * (smoothed_k_plus_1.state_mean - x_pred_k_plus_1);
        smoothed_states[k].state_covariance = fwd_k.state_covariance + Ck * (smoothed_k_plus_1.state_covariance - P_pred_k_plus_1) * Ck.transpose();
    }

    return smoothed_states;
}

FilterState KalmanFilter::getState() const {
    return FilterState{.state_mean = x_, .state_covariance = P_};
}

std::unique_ptr<IFilter> KalmanFilter::clone() const {
    return std::make_unique<KalmanFilter>(*this);
}

// Add the implementation for our new method
std::unique_ptr<IFilter> KalmanFilter::createBackwardFilter() const {
    if (F_inv_.rows() == 0 || F_inv_.cols() == 0) {
        // If the matrix isn't invertible, we can't create a backward filter.
        // Return nullptr or throw an exception.
        return nullptr;
    }
    // Create a new KalmanFilter, but feed it the INVERSE matrices.
    // Its "forward" is our "backward".
    auto backward_filter = std::make_unique<KalmanFilter>(F_inv_, H_, Q_backward_, R_);
    return backward_filter;
}

/*
std::optional<FilterState> KalmanFilter::predictPrevious(FilterState const & current_state) {
    if (F_inv_.rows() == 0 || F_inv_.cols() == 0) {
        return std::nullopt;
    }
    Eigen::VectorXd x_prev = F_inv_ * current_state.state_mean;
    // Covariance backward propagation: P_prev = F_inv * P * F_inv^T + Q_backward
    Eigen::MatrixXd P_prev = F_inv_ * current_state.state_covariance * F_inv_.transpose() + Q_backward_;
    // Force symmetry
    P_prev = 0.5 * (P_prev + P_prev.transpose());
    // Clamp negative eigenvalues to small positive to maintain PSD
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(P_prev);
    Eigen::VectorXd eig = es.eigenvalues();
    Eigen::MatrixXd V = es.eigenvectors();
    double const eps = 1e-9;
    for (int i = 0; i < eig.size(); ++i) {
        if (eig[i] < eps) eig[i] = eps;
    }
    Eigen::MatrixXd P_prev_psd = V * eig.asDiagonal() * V.transpose();
    return FilterState{.state_mean = x_prev, .state_covariance = P_prev_psd};
}
*/

}// namespace StateEstimation

1	#include "KalmanFilter.hpp"
2
3
4	#include <iostream>
5	#include <stdexcept>
6
7	namespace StateEstimation {
8
9	KalmanFilter::KalmanFilter(Eigen::MatrixXd const & F, Eigen::MatrixXd const & H,	15✔
10	Eigen::MatrixXd const & Q, Eigen::MatrixXd const & R)	15✔
11	: F_(F),	15✔
12	H_(H),	15✔
13	Q_(Q),	15✔
14	R_(R),	15✔
15	x_(Eigen::VectorXd::Zero(F.rows())),	15✔
16	P_(Eigen::MatrixXd::Identity(F.rows(), F.rows())),	15✔
17	F_inv_(Eigen::MatrixXd::Zero(F.rows(), F.cols())),	15✔
18	Q_backward_(Q.rows(), Q.cols()) {	30✔
19	if (F_.rows() == F_.cols()) {	15✔
20	// Compute inverse if well-conditioned; allow Eigen to throw if singular
21	F_inv_ = F_.inverse();	15✔
22	// Backward process noise: Q_b = F_inv * Q * F_inv^T (approximate mapping)
23	Q_backward_ = F_inv_ * Q_ * F_inv_.transpose();	15✔
24	// Ensure symmetry
25	Q_backward_ = 0.5 * (Q_backward_ + Q_backward_.transpose());	15✔
26	}
27	}	15✔
28
29	void KalmanFilter::initialize(FilterState const & initial_state) {	1,225✔
30	x_ = initial_state.state_mean;	1,225✔
31	P_ = initial_state.state_covariance;	1,225✔
32	}	1,225✔
33
34	FilterState KalmanFilter::predict() {	5,720✔
35	x_ = F_ * x_;	5,720✔
36	P_ = F_ * P_ * F_.transpose() + Q_;	5,720✔
37	return FilterState{.state_mean = x_, .state_covariance = P_};	5,720✔
38	}
39
40	FilterState KalmanFilter::update(FilterState const & predicted_state, Measurement const & measurement) {	3,784✔
41	return update(predicted_state, measurement, 1.0);	3,784✔
42	}
43
44	FilterState KalmanFilter::update(FilterState const & predicted_state, Measurement const & measurement, double noise_scale_factor) {	3,784✔
45	Eigen::VectorXd const & z = measurement.feature_vector;	3,784✔
46
47	// Use the predicted state passed in
48	Eigen::VectorXd const x_pred = predicted_state.state_mean;	3,784✔
49	Eigen::MatrixXd const P_pred = predicted_state.state_covariance;	3,784✔
50
51	// Scale the measurement noise matrix R by the noise scale factor
52	Eigen::MatrixXd const R_scaled = noise_scale_factor * R_;	3,784✔
53
54	Eigen::VectorXd const y = z - H_ * x_pred; // Innovation or residual	3,784✔
55	Eigen::MatrixXd S = H_ * P_pred * H_.transpose() + R_scaled; // Innovation covariance	3,784✔
56	Eigen::MatrixXd K = P_pred * H_.transpose() * S.inverse();// Kalman gain	3,784✔
57
58	x_ = x_pred + K * y;	3,784✔
59	P_ = (Eigen::MatrixXd::Identity(x_.size(), x_.size()) - K * H_) * P_pred;	3,784✔
60
61	return FilterState{.state_mean = x_, .state_covariance = P_};	7,568✔
62	}	3,784✔
63
64	std::vector<FilterState> KalmanFilter::smooth(std::vector<FilterState> const & forward_states) {	178✔
65	if (forward_states.empty()) {	178✔
66	return {};	×
67	}
68
69	std::vector<FilterState> smoothed_states = forward_states;	178✔
70
71	// The backward pass (Rauch-Tung-Striebel smoother)
72	// Propagate information backward from k+1 to k
73	for (int k = static_cast<int>(forward_states.size()) - 2; k >= 0; --k) {	1,755✔
74	FilterState const & fwd_k = forward_states[k];	1,577✔
75	FilterState const & smoothed_k_plus_1 = smoothed_states[k + 1];	1,577✔
76
77	// Prediction for k+1 based on forward estimate at k
78	Eigen::VectorXd x_pred_k_plus_1 = F_ * fwd_k.state_mean;	1,577✔
79	Eigen::MatrixXd P_pred_k_plus_1 = F_ * fwd_k.state_covariance * F_.transpose() + Q_;	1,577✔
80
81	// Smoother gain: C_k = P_k * F^T * P_pred^{-1}
82	Eigen::MatrixXd Ck = fwd_k.state_covariance * F_.transpose() * P_pred_k_plus_1.inverse();	1,577✔
83
84	// Smoothed estimate at k incorporating information from future (k+1)
85	smoothed_states[k].state_mean = fwd_k.state_mean + Ck * (smoothed_k_plus_1.state_mean - x_pred_k_plus_1);	1,577✔
86	smoothed_states[k].state_covariance = fwd_k.state_covariance + Ck * (smoothed_k_plus_1.state_covariance - P_pred_k_plus_1) * Ck.transpose();	1,577✔
87	}	1,577✔
88
89	return smoothed_states;	178✔
90	}	178✔
91
92	FilterState KalmanFilter::getState() const {	4,162✔
93	return FilterState{.state_mean = x_, .state_covariance = P_};	4,162✔
94	}
95
96	std::unique_ptr<IFilter> KalmanFilter::clone() const {	1,225✔
97	return std::make_unique<KalmanFilter>(*this);	1,225✔
98	}
99
100	// Add the implementation for our new method
UNCOV 101	std::unique_ptr<IFilter> KalmanFilter::createBackwardFilter() const {	×
UNCOV 102	if (F_inv_.rows() == 0 \|\| F_inv_.cols() == 0) {	×
103	// If the matrix isn't invertible, we can't create a backward filter.
104	// Return nullptr or throw an exception.
UNCOV 105	return nullptr;	×
106	}
107	// Create a new KalmanFilter, but feed it the INVERSE matrices.
108	// Its "forward" is our "backward".
UNCOV 109	auto backward_filter = std::make_unique<KalmanFilter>(F_inv_, H_, Q_backward_, R_);	×
UNCOV 110	return backward_filter;	×
UNCOV 111	}	×
112
113	/*
114	std::optional<FilterState> KalmanFilter::predictPrevious(FilterState const & current_state) {
115	if (F_inv_.rows() == 0 \|\| F_inv_.cols() == 0) {
116	return std::nullopt;
117	}
118	Eigen::VectorXd x_prev = F_inv_ * current_state.state_mean;
119	// Covariance backward propagation: P_prev = F_inv * P * F_inv^T + Q_backward
120	Eigen::MatrixXd P_prev = F_inv_ * current_state.state_covariance * F_inv_.transpose() + Q_backward_;
121	// Force symmetry
122	P_prev = 0.5 * (P_prev + P_prev.transpose());
123	// Clamp negative eigenvalues to small positive to maintain PSD
124	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(P_prev);
125	Eigen::VectorXd eig = es.eigenvalues();
126	Eigen::MatrixXd V = es.eigenvectors();
127	double const eps = 1e-9;
128	for (int i = 0; i < eig.size(); ++i) {
129	if (eig[i] < eps) eig[i] = eps;
130	}
131	Eigen::MatrixXd P_prev_psd = V * eig.asDiagonal() * V.transpose();
132	return FilterState{.state_mean = x_prev, .state_covariance = P_prev_psd};
133	}
134	*/
135
136	}// namespace StateEstimation

paulmthompson / WhiskerToolbox / 18389801194

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous