18246927847

Committed 04 Oct 2025 04:44PM UTC coverage: 71.826% (+0.6%) from 71.188%

Build # 18246927847

Build Type

push

github

Committed by

paulmthompson

Commit Message

refactor out media producer consumer pipeline

Run Details

0 of 120 new or added lines in 2 files covered. (0.0%)

646 existing lines in 14 files now uncovered.

48895 of 68074 relevant lines covered (71.83%)

1193.51 hits per line

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

93.65

/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;
    FilterState & last_smoothed = smoothed_states.back();

    // The backward pass
    for (int k = static_cast<int>(forward_states.size()) - 2; k >= 0; --k) {
        FilterState const & prev_forward = forward_states[k];

        // Prediction for the next state based on the previous forward state
        Eigen::VectorXd x_pred_next = F_ * prev_forward.state_mean;
        Eigen::MatrixXd P_pred_next = F_ * prev_forward.state_covariance * F_.transpose() + Q_;

        // Smoother gain
        Eigen::MatrixXd Ck = prev_forward.state_covariance * F_.transpose() * P_pred_next.inverse();

        // Update the state and covariance
        smoothed_states[k].state_mean = prev_forward.state_mean + Ck * (last_smoothed.state_mean - x_pred_next);
        smoothed_states[k].state_covariance = prev_forward.state_covariance + Ck * (last_smoothed.state_covariance - P_pred_next) * Ck.transpose();

        last_smoothed = smoothed_states[k];
    }

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

paulmthompson / WhiskerToolbox / 18246927847

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