Skip to content

Commit

Permalink
Incorporate direct process noise
Browse files Browse the repository at this point in the history 
Signed-off-by: Markus Mayer <widemeadows@gmail.com>
  • Loading branch information
@sunsided
sunsided committed Jun 22, 2024
1 parent 7a0e3a8 commit f27ed19
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 10 deletions.
17 changes: 13 additions & 4 deletions crates/minikalman/src/kalman/extended.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -363,6 +363,7 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
A: StateTransitionMatrix<STATES, T>,
PX: PredictedStateEstimateVector<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
F: FnMut(&X, &mut PX),
Expand Down Expand Up @@ -407,6 +408,7 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
A: StateTransitionMatrix<STATES, T>,
PX: PredictedStateEstimateVector<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
F: FnMut(&X, &mut PX),
Expand Down Expand Up @@ -442,21 +444,24 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
where
A: StateTransitionMatrix<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
{
// matrices and vectors
let A = self.A.as_matrix();
let P = self.P.as_matrix_mut();
let Q = self.Q.as_matrix();

// temporaries
let P_temp = self.temp_P.as_matrix_mut();

// Predict next covariance using system dynamics (without control)

// P = A*P*Aᵀ
// P = A*P*Aᵀ + Q
A.mult(P, P_temp); // temp = A*P
P_temp.mult_transb(A, P); // P = temp*Aᵀ
Q.add_inplace_b(P); // P = P + Q
}

#[allow(non_snake_case)]
Expand All @@ -465,12 +470,14 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
where
A: StateTransitionMatrix<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
{
// matrices and vectors
let A = self.A.as_matrix();
let P = self.P.as_matrix_mut();
let Q = self.Q.as_matrix();

// temporaries
let P_temp = self.temp_P.as_matrix_mut();
Expand All @@ -479,11 +486,12 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
// P = A*P*Aᵀ * 1/lambda^2

// lambda = 1/lambda^2
let lambda = lambda.mul(lambda).recip(); // TODO: This should be precalculated, e.g. using set_lambda(...);
let factor = lambda.mul(lambda).recip(); // TODO: This should be precalculated, e.g. using set_lambda(...);

// P = A*P*A'
// P = A*P*A' * 1/(lambda^2) + Q
A.mult(P, P_temp); // temp = A*P
P_temp.multscale_transb(A, lambda, P); // P = temp*A' * 1/(lambda^2)
P_temp.multscale_transb(A, factor, P); // P = temp*A' * 1/(lambda^2)
Q.add_inplace_b(P); // P = P + Q
}

/// Performs the nonlinear measurement update step.
Expand Down Expand Up @@ -602,6 +610,7 @@ where
A: StateTransitionMatrix<STATES, T>,
PX: PredictedStateEstimateVector<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
{
Expand Down
21 changes: 15 additions & 6 deletions crates/minikalman/src/kalman/regular.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -302,6 +302,7 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
A: StateTransitionMatrix<STATES, T>,
PX: PredictedStateEstimateVector<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
{
Expand All @@ -310,7 +311,7 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
self.predict_x();

//* Predict next covariance using system dynamics and control
//* P = A*P*Aᵀ
//* P = A*P*Aᵀ + Q
self.predict_P();
}

Expand Down Expand Up @@ -406,6 +407,7 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
A: StateTransitionMatrix<STATES, T>,
PX: PredictedStateEstimateVector<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
{
Expand All @@ -414,7 +416,7 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
self.predict_x();

// Predict next covariance using system dynamics and control
// P = A*P*Aᵀ * 1/lambda^2
// P = A*P*Aᵀ * 1/lambda^2 + Q
self.predict_P_tuned(lambda);
}

Expand Down Expand Up @@ -448,21 +450,24 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
where
A: StateTransitionMatrix<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
{
// matrices and vectors
let A = self.A.as_matrix();
let P = self.P.as_matrix_mut();
let Q = self.Q.as_matrix();

// temporaries
let P_temp = self.temp_P.as_matrix_mut();

// Predict next covariance using system dynamics (without control)

// P = A*P*Aᵀ
// P = A*P*Aᵀ + Q
A.mult(P, P_temp); // temp = A*P
P_temp.mult_transb(A, P); // P = temp*Aᵀ
Q.add_inplace_b(P); // P = P + Q
}

#[allow(non_snake_case)]
Expand All @@ -471,12 +476,14 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
where
A: StateTransitionMatrix<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
{
// matrices and vectors
let A = self.A.as_matrix();
let P = self.P.as_matrix_mut();
let Q = self.Q.as_matrix();

// temporaries
let P_temp = self.temp_P.as_matrix_mut();
Expand All @@ -485,11 +492,12 @@ impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
// P = A*P*Aᵀ * 1/lambda^2

// lambda = 1/lambda^2
let lambda = lambda.mul(lambda).recip(); // TODO: This should be precalculated, e.g. using set_lambda(...);
let factor = lambda.mul(lambda).recip(); // TODO: This should be precalculated, e.g. using set_lambda(...);

// P = A*P*A'
// P = A*P*A' * 1/(lambda^2) + Q
A.mult(P, P_temp); // temp = A*P
P_temp.multscale_transb(A, lambda, P); // P = temp*A' * 1/(lambda^2)
P_temp.multscale_transb(A, factor, P); // P = temp*A' * 1/(lambda^2)
Q.add_inplace_b(P); // P = P + Q
}

/// Applies a control input.
Expand Down Expand Up @@ -711,6 +719,7 @@ where
A: StateTransitionMatrix<STATES, T>,
PX: PredictedStateEstimateVector<STATES, T>,
P: EstimateCovarianceMatrix<STATES, T>,
Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
TempP: TemporaryStateMatrix<STATES, T>,
T: MatrixDataType,
{
Expand Down

0 comments on commit f27ed19

Please sign in to comment.