Skip to content

Commit a451577

Browse files
youtalkyoutalk
authored
perf(autoware_kalman_filter): use Cholesky solve in update() with full guard-branch test coverage (#1094)
* test(autoware_kalman_filter): cover validation and guard branches Add unit tests for the previously-untested KalmanFilter dimension validation FALSE branches (init/predict/update), the state and covariance getters, and a closed-form numerical-equivalence update check on a correlated covariance. Add TimeDelayKalmanFilter::updateWithDelay tests for the non-column-vector, non-square-R, R/C-row-mismatch, and non-positive-definite (LLT failure) rejection paths, capturing stderr so test output stays clean. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> * perf(autoware_kalman_filter): solve innovation covariance via Cholesky in update() Mirror the optimization already applied to TimeDelayKalmanFilter::updateWithDelay in the base KalmanFilter::update: compute the Kalman gain with a Cholesky (LLT) solve of the innovation covariance S = C P C^T + R instead of an explicit matrix inverse. This is faster and numerically more stable, and a failed decomposition now rejects updates whose innovation covariance is not positive definite, which the explicit inverse silently accepted with a meaningless gain (potentially corrupting downstream state in autoware_ekf_localizer). Also use noalias() in predict(x_next, A, Q) to drop the intermediate Eigen temporaries of A P A^T in that hot per-cycle path. The public bool API is unchanged; behavior is identical for valid inputs (verified against the closed-form solution) and fails safe for non-positive-definite ones. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> * test(autoware_kalman_filter): address review feedback - assert update against hand-computed numeric literals instead of a re-typed copy of the solver formula - strengthen the two-arg init test to verify stored state/covariance feed the filter math, keeping getXelement coverage Refs: #1096 Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> * test(autoware_kalman_filter): de-tautologize the kf end-to-end test (#78) Replace the recomputed-with-the-same-Kalman-formula expectations in TEST(kalman_filter, kf) with independent hand-computed numeric literals for the predict and update steps (A=B=C=I, Q=0.01I, R=0.09I), with the derivations written out in comments. The previous expectations re-typed the implementation's own predict/update expressions, so a wrong formula would have passed in both places. Refs: #1096 Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> * test(autoware_kalman_filter): drop trivial init getter/setter test (#88) interimadd flagged the two-argument init test as covering only straightforward getters/setters and empty values, with no logic to break. The follow-up commit had reworked it into init_state_cov_feed_into_filter_math, but it still ultimately exercises init() storing x/P round-tripped through getters, so it adds little value. Remove it entirely per the reviewer's request. The remaining tests already assert against hand-computed numeric literals independent of the SUT (update_matches_closed_form_with_correlated_covariance) and check only the boolean return of updateWithDelay for rejection paths, so the other two review points are already satisfied on the PR head. Refs: #1096 Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> * fix(autoware_kalman_filter): log LLT failure in update() like updateWithDelay Address review: KalmanFilter::update() rejected non-positive-definite innovation covariance silently (returned false with no message), an observability asymmetry with TimeDelayKalmanFilter::updateWithDelay. Emit the same std::cerr diagnostic so a caller that gets false can tell why the update was rejected. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> --------- Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
1 parent a0c2180 commit a451577

3 files changed

Lines changed: 291 additions & 31 deletions

File tree

‎common/autoware_kalman_filter/src/kalman_filter.cpp‎

Lines changed: 27 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,10 @@
1414

1515
#include "autoware/kalman_filter/kalman_filter.hpp"
1616

17+
#include <Eigen/Cholesky>
18+
19+
#include <iostream>
20+
1721
namespace autoware::kalman_filter
1822
{
1923
KalmanFilter::KalmanFilter(
@@ -95,7 +99,11 @@ bool KalmanFilter::predict(
9599
return false;
96100
}
97101
x_ = x_next;
98-
P_ = A * P_ * A.transpose() + Q;
102+
// P_ = A P_ A^T + Q, evaluated through a temporary so noalias() can drop the intermediate
103+
// Eigen products in this hot per-cycle path.
104+
const Eigen::MatrixXd AP = A * P_;
105+
P_.noalias() = AP * A.transpose();
106+
P_ += Q;
99107
return true;
100108
}
101109
bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A)
@@ -128,14 +136,29 @@ bool KalmanFilter::update(
128136
return false;
129137
}
130138
const Eigen::MatrixXd PCT = P_ * C.transpose();
131-
const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse());
139+
140+
// Innovation covariance S = C P C^T + R.
141+
Eigen::MatrixXd S = R;
142+
S.noalias() += C * PCT;
143+
144+
// Compute the Kalman gain K = PCT * S^-1 via a Cholesky (LLT) solve rather than an explicit
145+
// inverse: it is faster and numerically more stable, and a failed decomposition rejects updates
146+
// whose innovation covariance is not positive definite (which would otherwise corrupt the state).
147+
// S is symmetric, so K^T solves S * K^T = PCT^T.
148+
const Eigen::LLT<Eigen::MatrixXd> llt_of_s(S);
149+
if (llt_of_s.info() != Eigen::Success) {
150+
std::cerr << "LLT decomposition failed. S matrix might not be positive definite." << std::endl;
151+
return false;
152+
}
153+
const Eigen::MatrixXd K = llt_of_s.solve(PCT.transpose()).transpose();
132154

133155
if (K.array().isNaN().any() || K.array().isInf().any()) {
134156
return false;
135157
}
136158

137-
x_ = x_ + K * (y - y_pred);
138-
P_ = P_ - K * (C * P_);
159+
x_.noalias() += K * (y - y_pred);
160+
const Eigen::MatrixXd CP = C * P_;
161+
P_.noalias() -= K * CP;
139162
return true;
140163
}
141164

‎common/autoware_kalman_filter/test/test_kalman_filter.cpp‎

Lines changed: 209 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,21 @@
1818

1919
using autoware::kalman_filter::KalmanFilter;
2020

21+
namespace
22+
{
23+
// Build a minimal, valid 2-state filter (x_ is 2x1, P_ is 2x2) for exercising guard branches.
24+
KalmanFilter make_initialized_filter()
25+
{
26+
Eigen::MatrixXd x(2, 1);
27+
x << 1.0, 2.0;
28+
Eigen::MatrixXd p(2, 2);
29+
p << 1.0, 0.0, 0.0, 1.0;
30+
KalmanFilter kf;
31+
kf.init(x, p);
32+
return kf;
33+
}
34+
} // namespace
35+
2136
TEST(kalman_filter, kf)
2237
{
2338
KalmanFilter kf_;
@@ -46,49 +61,48 @@ TEST(kalman_filter, kf)
4661
// Initialize the filter and check if initialization was successful
4762
EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t));
4863

49-
// Perform prediction
64+
// Perform prediction with A = B = I, so x_pred = x + u and P_pred = P + Q.
65+
// x_pred = [1 + 0.1, 2 + 0.1] = [1.1, 2.1]
66+
// P_pred = I + 0.01 I (diagonal) = diag(1.01, 1.01)
5067
Eigen::MatrixXd u_t(2, 1);
5168
u_t << 0.1, 0.1;
5269
EXPECT_TRUE(kf_.predict(u_t));
5370

54-
// Check the updated state and covariance matrix
55-
Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t;
56-
Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t;
57-
71+
// Check the predicted state and covariance against hand-computed literals.
5872
Eigen::MatrixXd x_predict;
5973
kf_.getX(x_predict);
6074
Eigen::MatrixXd P_predict;
6175
kf_.getP(P_predict);
6276

63-
EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5);
64-
EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5);
65-
EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5);
66-
EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5);
77+
EXPECT_NEAR(x_predict(0, 0), 1.1, 1e-5);
78+
EXPECT_NEAR(x_predict(1, 0), 2.1, 1e-5);
79+
EXPECT_NEAR(P_predict(0, 0), 1.01, 1e-5);
80+
EXPECT_NEAR(P_predict(1, 1), 1.01, 1e-5);
6781

68-
// Perform update
82+
// Perform update with C = I, R = 0.09 I, y = [1.05, 2.05].
83+
// S = R + C P_pred C^T = diag(1.01 + 0.09) = diag(1.1)
84+
// K = P_pred C^T S^-1 = diag(1.01 / 1.1) = diag(0.918181..)
85+
// x_up = x_pred + K (y - x_pred)
86+
// = 1.1 + (1.01/1.1)*(1.05 - 1.1) = 1.05409090909.. (dim 0)
87+
// = 2.1 + (1.01/1.1)*(2.05 - 2.1) = 2.05409090909.. (dim 1)
88+
// P_up = P_pred - K (C P_pred)
89+
// = 1.01 - (1.01/1.1)*1.01 = 0.08263636363.. (diagonal)
6990
Eigen::MatrixXd y_t(2, 1);
7091
y_t << 1.05, 2.05;
7192
EXPECT_TRUE(kf_.update(y_t));
7293

73-
// Check the updated state and covariance matrix
74-
const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose();
75-
const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse());
76-
const Eigen::MatrixXd y_pred = C_t * x_predict_expected;
77-
Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred);
78-
Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected);
79-
8094
Eigen::MatrixXd x_update;
8195
kf_.getX(x_update);
8296
Eigen::MatrixXd P_update;
8397
kf_.getP(P_update);
8498

85-
EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5);
86-
EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5);
87-
EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5);
88-
EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5);
89-
90-
// Add tests to cover missed lines
99+
EXPECT_NEAR(x_update(0, 0), 1.0540909090909092, 1e-5);
100+
EXPECT_NEAR(x_update(1, 0), 2.0540909090909087, 1e-5);
101+
EXPECT_NEAR(P_update(0, 0), 0.08263636363636362, 1e-5);
102+
EXPECT_NEAR(P_update(1, 1), 0.08263636363636362, 1e-5);
91103

104+
// Exercise the value constructor and the explicit-A predict overload. With a fresh state
105+
// initialized to (x_t, P_t) and A = I, Q = 0.01 I, the covariance becomes P + Q = diag(1.01).
92106
KalmanFilter kf_new(x_t, A_t, B_t, C_t, Q_t, R_t, P_t);
93107
kf_new.init(x_t, P_t);
94108
kf_new.setA(A_t);
@@ -97,11 +111,179 @@ TEST(kalman_filter, kf)
97111
kf_new.setQ(Q_t);
98112
kf_new.setR(R_t);
99113

100-
EXPECT_TRUE(kf_new.predict(x_predict_expected, A_t));
101-
P_predict_expected = A_t * P_t * A_t.transpose() + Q_t;
114+
Eigen::MatrixXd x_next(2, 1);
115+
x_next << 1.1, 2.1;
116+
EXPECT_TRUE(kf_new.predict(x_next, A_t));
102117
kf_new.getP(P_predict);
103-
EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5);
104-
EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5);
118+
EXPECT_NEAR(P_predict(0, 0), 1.01, 1e-5);
119+
EXPECT_NEAR(P_predict(1, 1), 1.01, 1e-5);
120+
}
121+
122+
TEST(kalman_filter, init_full_rejects_empty_matrix)
123+
{
124+
KalmanFilter kf;
125+
const Eigen::MatrixXd x = Eigen::MatrixXd::Zero(2, 1);
126+
const Eigen::MatrixXd m = Eigen::MatrixXd::Identity(2, 2);
127+
const Eigen::MatrixXd empty; // 0x0
128+
129+
// Each empty argument independently makes the full init return false.
130+
EXPECT_FALSE(kf.init(empty, m, m, m, m, m, m)); // x empty
131+
EXPECT_FALSE(kf.init(x, empty, m, m, m, m, m)); // A empty
132+
EXPECT_FALSE(kf.init(x, m, empty, m, m, m, m)); // B empty
133+
EXPECT_FALSE(kf.init(x, m, m, empty, m, m, m)); // C empty
134+
EXPECT_FALSE(kf.init(x, m, m, m, empty, m, m)); // Q empty
135+
EXPECT_FALSE(kf.init(x, m, m, m, m, empty, m)); // R empty
136+
EXPECT_FALSE(kf.init(x, m, m, m, m, m, empty)); // P empty
137+
EXPECT_TRUE(kf.init(x, m, m, m, m, m, m)); // all valid
138+
}
139+
140+
TEST(kalman_filter, init_state_cov_rejects_empty_matrix)
141+
{
142+
KalmanFilter kf;
143+
const Eigen::MatrixXd x = Eigen::MatrixXd::Zero(2, 1);
144+
const Eigen::MatrixXd p = Eigen::MatrixXd::Identity(2, 2);
145+
const Eigen::MatrixXd empty;
146+
147+
EXPECT_FALSE(kf.init(empty, p));
148+
EXPECT_FALSE(kf.init(x, empty));
149+
EXPECT_TRUE(kf.init(x, p));
150+
}
151+
152+
TEST(kalman_filter, predict_with_x_and_a_rejects_dimension_mismatch)
153+
{
154+
const Eigen::MatrixXd a2 = Eigen::MatrixXd::Identity(2, 2);
155+
const Eigen::MatrixXd q2 = Eigen::MatrixXd::Identity(2, 2);
156+
const Eigen::MatrixXd x_next = Eigen::MatrixXd::Zero(2, 1);
157+
158+
{ // x_.rows() != x_next.rows()
159+
auto kf = make_initialized_filter();
160+
EXPECT_FALSE(kf.predict(Eigen::MatrixXd::Zero(3, 1), a2, q2));
161+
}
162+
{ // A.cols() != P_.rows()
163+
auto kf = make_initialized_filter();
164+
EXPECT_FALSE(kf.predict(x_next, Eigen::MatrixXd::Zero(2, 3), q2));
165+
}
166+
{ // Q.cols() != Q.rows()
167+
auto kf = make_initialized_filter();
168+
EXPECT_FALSE(kf.predict(x_next, a2, Eigen::MatrixXd::Zero(2, 3)));
169+
}
170+
{ // A.rows() != Q.cols()
171+
auto kf = make_initialized_filter();
172+
EXPECT_FALSE(kf.predict(x_next, Eigen::MatrixXd::Zero(3, 2), q2));
173+
}
174+
}
175+
176+
TEST(kalman_filter, predict_with_input_rejects_dimension_mismatch)
177+
{
178+
const Eigen::MatrixXd a2 = Eigen::MatrixXd::Identity(2, 2);
179+
const Eigen::MatrixXd b2 = Eigen::MatrixXd::Identity(2, 2);
180+
const Eigen::MatrixXd q2 = Eigen::MatrixXd::Identity(2, 2);
181+
const Eigen::MatrixXd u2 = Eigen::MatrixXd::Zero(2, 1);
182+
183+
{ // A.cols() != x_.rows()
184+
auto kf = make_initialized_filter();
185+
EXPECT_FALSE(kf.predict(u2, Eigen::MatrixXd::Zero(2, 3), b2, q2));
186+
}
187+
{ // B.cols() != u.rows()
188+
auto kf = make_initialized_filter();
189+
EXPECT_FALSE(kf.predict(u2, a2, Eigen::MatrixXd::Zero(2, 3), q2));
190+
}
191+
}
192+
193+
TEST(kalman_filter, update_full_rejects_dimension_mismatch)
194+
{
195+
const Eigen::MatrixXd y2 = Eigen::MatrixXd::Zero(2, 1);
196+
const Eigen::MatrixXd c2 = Eigen::MatrixXd::Identity(2, 2);
197+
const Eigen::MatrixXd r2 = Eigen::MatrixXd::Identity(2, 2);
198+
199+
{ // P_.cols() != C.cols()
200+
auto kf = make_initialized_filter();
201+
EXPECT_FALSE(kf.update(y2, y2, Eigen::MatrixXd::Identity(2, 3), r2));
202+
}
203+
{ // R not square
204+
auto kf = make_initialized_filter();
205+
EXPECT_FALSE(kf.update(y2, y2, c2, Eigen::MatrixXd::Zero(2, 3)));
206+
}
207+
{ // R.rows() != C.rows()
208+
auto kf = make_initialized_filter();
209+
EXPECT_FALSE(kf.update(y2, y2, c2, Eigen::MatrixXd::Identity(3, 3)));
210+
}
211+
{ // y.rows() != y_pred.rows()
212+
auto kf = make_initialized_filter();
213+
EXPECT_FALSE(kf.update(y2, Eigen::MatrixXd::Zero(3, 1), c2, r2));
214+
}
215+
{ // y.rows() != C.rows()
216+
auto kf = make_initialized_filter();
217+
const Eigen::MatrixXd y3 = Eigen::MatrixXd::Zero(3, 1);
218+
EXPECT_FALSE(kf.update(y3, y3, c2, r2));
219+
}
220+
}
221+
222+
TEST(kalman_filter, update_rejects_observation_dimension_mismatch)
223+
{
224+
auto kf = make_initialized_filter();
225+
const Eigen::MatrixXd y = Eigen::MatrixXd::Zero(2, 1);
226+
const Eigen::MatrixXd c_wrong = Eigen::MatrixXd::Identity(2, 3); // C.cols() != x_.rows()
227+
const Eigen::MatrixXd r = Eigen::MatrixXd::Identity(2, 2);
228+
EXPECT_FALSE(kf.update(y, c_wrong, r));
229+
}
230+
231+
TEST(kalman_filter, update_matches_closed_form_with_correlated_covariance)
232+
{
233+
// Exercises the gain computation on a non-diagonal, well-conditioned problem and pins the
234+
// result to hand-computed numeric literals, guarding any change to the solver.
235+
//
236+
// Hand derivation for x = [1, -2]^T, P = [[2, 0.3], [0.3, 1.5]], C = [1, 0.5], R = [0.2],
237+
// y = [0.4]:
238+
// y_pred = C x = 1*1 + 0.5*(-2) = 0
239+
// P C^T = [2*1 + 0.3*0.5, 0.3*1 + 1.5*0.5]^T = [2.15, 1.05]^T
240+
// S = R + C P C^T = 0.2 + (1*2.15 + 0.5*1.05) = 2.875
241+
// K = P C^T / S = [2.15/2.875, 1.05/2.875]^T = [0.74782608.., 0.36521739..]^T
242+
// x_new = x + K (y - y_pred) = x + 0.4 * K = [1.29913043.., -1.85391304..]^T
243+
// P_new = P - K (C P) = [[ 0.39217391.., -0.48521739..],
244+
// [-0.48521739.., 1.11652173..]]
245+
KalmanFilter kf;
246+
Eigen::MatrixXd x(2, 1);
247+
x << 1.0, -2.0;
248+
Eigen::MatrixXd p(2, 2);
249+
p << 2.0, 0.3, 0.3, 1.5; // symmetric positive-definite with off-diagonal correlation
250+
ASSERT_TRUE(kf.init(x, p));
251+
252+
Eigen::MatrixXd c(1, 2);
253+
c << 1.0, 0.5;
254+
Eigen::MatrixXd r(1, 1);
255+
r << 0.2;
256+
Eigen::MatrixXd y(1, 1);
257+
y << 0.4;
258+
259+
ASSERT_TRUE(kf.update(y, c, r));
260+
Eigen::MatrixXd x_out;
261+
kf.getX(x_out);
262+
Eigen::MatrixXd p_out;
263+
kf.getP(p_out);
264+
265+
EXPECT_NEAR(x_out(0, 0), 1.2991304347826087, 1e-10);
266+
EXPECT_NEAR(x_out(1, 0), -1.853913043478261, 1e-10);
267+
EXPECT_NEAR(p_out(0, 0), 0.3921739130434785, 1e-10);
268+
EXPECT_NEAR(p_out(0, 1), -0.48521739130434777, 1e-10);
269+
EXPECT_NEAR(p_out(1, 0), -0.48521739130434777, 1e-10);
270+
EXPECT_NEAR(p_out(1, 1), 1.1165217391304347, 1e-10);
271+
}
272+
273+
TEST(kalman_filter, update_rejects_non_positive_definite_innovation_covariance)
274+
{
275+
// A negative-definite measurement covariance R makes the innovation covariance
276+
// S = R + C P C^T non-positive-definite. Such an update is invalid and must be rejected
277+
// to avoid corrupting the state, rather than silently applying a meaningless gain.
278+
KalmanFilter kf;
279+
const Eigen::MatrixXd x = Eigen::MatrixXd::Zero(2, 1);
280+
const Eigen::MatrixXd p = Eigen::MatrixXd::Identity(2, 2);
281+
ASSERT_TRUE(kf.init(x, p));
282+
283+
const Eigen::MatrixXd y = Eigen::MatrixXd::Zero(2, 1);
284+
const Eigen::MatrixXd c = Eigen::MatrixXd::Identity(2, 2);
285+
const Eigen::MatrixXd r = -2.0 * Eigen::MatrixXd::Identity(2, 2); // S = -I, non-PD
286+
EXPECT_FALSE(kf.update(y, c, r));
105287
}
106288

107289
int main(int argc, char * argv[])

0 commit comments

Comments
 (0)