Skip to content

Commit

Permalink
Correct Cholesky factor to have positive diagonal (#17)
Browse files Browse the repository at this point in the history
  • Loading branch information
@THargreaves
THargreaves authored Nov 20, 2024
1 parent 9db8d18 commit 86231fc
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 0 deletions.
23 changes: 23 additions & 0 deletions src/srkf.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,25 @@ function calc_upper_triangular_of_qr!(A)
R
end

"""
correct_cholesky_sign(R)
The upper triangle R of the QR decomposition doesn't necessarily have positive
elements on the diagonal elements. However, when used in the context of
a Cholesky decomposition the diagonal elements need to be positive. Hence,
the function corrects the sign of the diagonal elements when necessary.
"""
correct_cholesky_sign(R) = sign.(diag(R)) .* R

function correct_cholesky_sign!(R)
for i in axes(R,1)
if real(R[i,i]) < 0
R[i, :] = -R[i, :]
end
end
return R
end

# This implementation is based on
# https://github.com/rlabbe/filterpy/blob/master/filterpy/kalman/square_root.py
function calc_apriori_covariance(
Expand All @@ -23,6 +42,7 @@ function calc_apriori_covariance(
)
A = vcat(P.U * F', Q.U)
R = calc_upper_triangular_of_qr(A)
R = correct_cholesky_sign(R)
Cholesky(R, 'U', 0)
end

Expand Down Expand Up @@ -60,6 +80,7 @@ function calc_cross_cov_innovation_posterior(P::Cholesky, H, R::Cholesky, consid
num_y = size(R, 1)
M = create_matrix_for_qr(P, H, R)
RU = calc_upper_triangular_of_qr(M)
RU = correct_cholesky_sign(RU)
PHᵀ = extract_cross_covariance(RU, num_y)
S = extract_innovation_covariance(RU, num_y)
P_post = extract_posterior_covariance(RU, num_y, P, consider)
Expand Down Expand Up @@ -103,6 +124,7 @@ function time_update!(tu::SRKFTUIntermediate, x, P::Cholesky, F::Union{Number, A
tu.puft_vcat_q[1:size(F, 1),:] .= @~ P.U * F'
tu.puft_vcat_q[size(F, 1) + 1:end,:] .= Q.U
R = calc_upper_triangular_of_qr_inplace!(tu.p_apri, tu.puft_vcat_q, tu.qr_zeros, tu.qr_space)
correct_cholesky_sign!(R)
P_apri = Cholesky(R, 'U', 0)
KFTimeUpdate(x_apri, P_apri)
end
Expand Down Expand Up @@ -149,6 +171,7 @@ function measurement_update!(
M[dim_y + 1:end, 1:dim_y] .= @~ P.U * H'
M[dim_y + 1:end, dim_y + 1:end] .= P.U
RU = calc_upper_triangular_of_qr_inplace!(mu.R, M, mu.qr_zeros, mu.qr_space)
correct_cholesky_sign!(RU)
PHᵀ = (@view(RU[1:dim_y, dim_y + 1:end]))'
S = Cholesky(@view(RU[1:dim_y, 1:dim_y]), 'U', 0)
K = calc_kalman_gain!(mu.kalman_gain, PHᵀ, S.L)
Expand Down
4 changes: 4 additions & 0 deletions src/srukf.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,7 @@ function cov(χ::TransformedSigmaPoints, noise::Cholesky)
weight_0, weight_i = calc_cov_weights(χ.weight_params, (size(χ, 2) - 1) >> 1)
A = vcat(sqrt(weight_i) * χ.xi', noise.uplo === 'U' ? noise.U : noise.L')
R = calc_upper_triangular_of_qr!(A)
correct_cholesky_sign!(R)
S = Cholesky(R, 'U', 0)
if weight_0 < 0
P = lowrankdowndate(S, sqrt(abs(weight_0)) * χ.x0)
Expand All @@ -94,6 +95,7 @@ function cov!(res, qr_A, qr_zeros, qr_space, x0_temp, χ::TransformedSigmaPoints
qr_A[1:size(χ.xi, 2), :] .= sqrt(weight_i) .* χ.xi'
qr_A[size(χ.xi, 2) + 1:end, :] = noise.uplo === 'U' ? noise.U : noise.L'
R = calc_upper_triangular_of_qr_inplace!(res, qr_A, qr_zeros, qr_space)
correct_cholesky_sign!(R)
S = Cholesky(R, 'U', 0)
x0_temp .= sqrt(abs(weight_0)) .* χ.x0
if weight_0 < 0
Expand All @@ -108,6 +110,7 @@ function cov(χ::TransformedSigmaPoints, noise::Augment{<:Cholesky})
weight_0, weight_i = calc_cov_weights(χ.weight_params, (size(χ, 2) - 1) >> 1)
A = sqrt(weight_i) * χ.xi'
R = calc_upper_triangular_of_qr!(A)
correct_cholesky_sign!(R)
S = Cholesky(R, 'U', 0)
if weight_0 < 0
P = lowrankdowndate(S, sqrt(abs(weight_0)) * χ.x0)
Expand All @@ -121,6 +124,7 @@ function cov!(res, qr_A, qr_zeros, qr_space, x0_temp, χ::TransformedSigmaPoints
weight_0, weight_i = calc_cov_weights(χ.weight_params, (size(χ, 2) - 1) >> 1)
qr_A .= sqrt(weight_i) .* χ.xi'
R = calc_upper_triangular_of_qr_inplace!(res, qr_A, qr_zeros, qr_space)
correct_cholesky_sign!(R)
S = Cholesky(R, 'U', 0)
x0_temp .= sqrt(abs(weight_0)) .* χ.x0
if weight_0 < 0
Expand Down

2 comments on commit 86231fc

@zsoerenm
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@JuliaRegistrator register

@JuliaRegistrator
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Registration pull request created: JuliaRegistries/General/119814

Tip: Release Notes

Did you know you can add release notes too? Just add markdown formatted text underneath the comment after the text
"Release notes:" and it will be added to the registry PR, and if TagBot is installed it will also be added to the
release that TagBot creates. i.e.

@JuliaRegistrator register

Release notes:

## Breaking changes

- blah

To add them here just re-invoke and the PR will be updated.

Tagging

After the above pull request is merged, it is recommended that a tag is created on this repository for the registered package version.

This will be done automatically if the Julia TagBot GitHub Action is installed, or can be done manually through the github interface, or via:

git tag -a v0.1.5 -m "<description of version>" 86231fc3349465117b5b950ec9b0694cd97139fc
git push origin v0.1.5

Please sign in to comment.