├── .gitignore ├── Project.toml ├── QRkalman_filter.jl ├── README.md ├── comparison_script.jl └── kalman_filter.jl /.gitignore: -------------------------------------------------------------------------------- 1 | # Files generated by invoking Julia with --code-coverage 2 | *.jl.cov 3 | *.jl.*.cov 4 | 5 | # Files generated by invoking Julia with --track-allocation 6 | *.jl.mem 7 | 8 | # System-specific files and directories generated by the BinaryProvider and BinDeps packages 9 | # They contain absolute paths specific to the host computer, and so should not be committed 10 | deps/deps.jl 11 | deps/build.log 12 | deps/downloads/ 13 | deps/usr/ 14 | deps/src/ 15 | 16 | # Build artifacts for creating documentation generated by the Documenter package 17 | docs/build/ 18 | docs/site/ 19 | 20 | # File generated by Pkg, the package manager, based on a corresponding Project.toml 21 | # It records a fixed state of all packages used by the project. As such, it should not be 22 | # committed for packages, but should be committed for applications that require a static 23 | # environment. 24 | Manifest.toml 25 | -------------------------------------------------------------------------------- /Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" 3 | LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 4 | MATLAB = "10e44e05-a98a-55b3-a45b-ba969058deb6" 5 | -------------------------------------------------------------------------------- /QRkalman_filter.jl: -------------------------------------------------------------------------------- 1 | using LinearAlgebra 2 | 3 | # square root stuff 4 | function chol(A) 5 | # returns upper triangular Cholesky factorization of matrix A 6 | return cholesky(Symmetric(A)).U 7 | end 8 | function qrᵣ(A) 9 | # QR decomposition of A where only the upper triangular R is returned 10 | return qr(A).R 11 | end 12 | 13 | function sqrkalman_filter(μ,F,u,y,kf_sys) 14 | 15 | μ̄, F̄ = sqrkf_predict(μ,F,u,kf_sys) 16 | 17 | z, L = sqrkf_innovate(μ̄,F̄,y,kf_sys) 18 | 19 | μ₊, F₊ = sqrkf_update(μ̄,F̄,z,L,kf_sys) 20 | 21 | return μ₊, F₊ 22 | end 23 | 24 | function sqrkf_predict(μ,F₋,u,kf_sys) 25 | 26 | # get probem data from kf_sys 27 | A, B, ΓQ = kf_sys.A, kf_sys.B, kf_sys.ΓQ 28 | 29 | # predict one step 30 | μ̄ = A*μ + B*u 31 | F̄ = qrᵣ([F₋*A';ΓQ]) 32 | 33 | return μ̄, F̄ 34 | end 35 | 36 | function sqrkf_innovate(μ̄,F̄,y,kf_sys) 37 | 38 | # get probem data from kf_sys 39 | A, C, ΓR = kf_sys.A, kf_sys.C, kf_sys.ΓR 40 | 41 | # innovation 42 | z = y - C*μ̄ 43 | G = qrᵣ([F̄*C';ΓR]) 44 | 45 | # kalman gain 46 | L = ((F̄'*F̄*C')/G)/(G') 47 | 48 | return z, L 49 | end 50 | function sqrkf_update(μ̄,F̄,z,L,kf_sys) 51 | 52 | # problem data 53 | ΓR, C = kf_sys.ΓR, kf_sys.C 54 | 55 | # update (Joseph form for Σ) 56 | μ₊= μ̄ + L*z 57 | F₊= qrᵣ([F̄*(I - L*C)';ΓR*L']) 58 | 59 | return μ₊, F₊ 60 | end 61 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # QRKalmanFilter 2 | Square root Kalman Filter using only QR decompositions. 3 | 4 | Accompanying code for [this arxiv paper](https://arxiv.org/abs/2207.00669). 5 | -------------------------------------------------------------------------------- /comparison_script.jl: -------------------------------------------------------------------------------- 1 | using LinearAlgebra 2 | # using MATLAB 3 | include(joinpath(@__DIR__,"kalman_filter.jl")) 4 | include(joinpath(@__DIR__,"QRkalman_filter.jl")) 5 | 6 | function rando() 7 | end 8 | 9 | 10 | function run_example() 11 | # double integrator discrete dynamics 12 | dt = 0.1 13 | n, m, nu = 6, 6, 3 14 | A = I + [zeros(3,3) dt*I; zeros(3,6)] 15 | B = Array([.5*dt^2*I(3);dt*I(3)]) 16 | C = Array(I(6)) 17 | 18 | # noise covariances 19 | Q = Array(0.0001*I(6)) 20 | R = Array(.001*I(6)) 21 | kf_sys = (A = A, B = B, C = C, Q = Q, R = R) 22 | 23 | # sqrt Kalman Filter 24 | sqrkf_sys = (A = A, B = B, C = C, ΓQ = chol(Q), ΓR = chol(R)) 25 | 26 | # allocate 27 | N = 1000 28 | X = [zeros(n) for i = 1:N] 29 | Y = [zeros(m) for i = 1:N] 30 | U = [zeros(nu) for i = 1:N] 31 | μ = [zeros(n) for i = 1:N] 32 | Σ = [zeros(n,n) for i = 1:N] 33 | 34 | # initial conditions 35 | X[1] = [1;2;3;0;0;0] 36 | Y[1] = C*X[1] + sqrt(R)*randn(6) 37 | 38 | # KF initialization 39 | μ[1] = X[1] + .00001*randn(6) 40 | Σ[1] = .05*I(6) 41 | 42 | # QRKF initialization 43 | μsqr = deepcopy(μ) 44 | F = deepcopy(Σ) 45 | F[1] = chol(Σ[1]) 46 | 47 | # simulation 48 | for i = 1:N-1 49 | 50 | # control input 51 | t = dt*(i-1) 52 | U[i] = [sin(t);cos(t);.5*sin(2*t)] 53 | 54 | # propagate dynamics forward one step 55 | X[i+1] = A*X[i] + B*U[i] + sqrt(Q)*randn(6) 56 | 57 | # take masurement 58 | Y[i+1] = C*X[i+1] + sqrt(R)*randn(6) 59 | 60 | # Kalman Filter 61 | μ[i+1], Σ[i+1] = kalman_filter(μ[i],Σ[i],U[i],Y[i+1],kf_sys) 62 | 63 | # sqrt Kalman Filter 64 | @time μsqr[i+1], F[i+1] = sqrkalman_filter(μsqr[i],F[i],U[i],Y[i+1],sqrkf_sys) 65 | end 66 | @show "done" 67 | end 68 | 69 | 70 | run_example() 71 | -------------------------------------------------------------------------------- /kalman_filter.jl: -------------------------------------------------------------------------------- 1 | using LinearAlgebra 2 | 3 | 4 | function kalman_filter(μ,Σ,u,y,kf_sys) 5 | 6 | μ̄, Σ̄ = kf_predict(μ,Σ,u,kf_sys) 7 | 8 | z, L = kf_innovate(μ̄,Σ̄,y,kf_sys) 9 | 10 | μ₊, Σ₊ = kf_update(μ̄,Σ̄,z,L,kf_sys) 11 | 12 | return μ₊, Σ₊ 13 | end 14 | 15 | function kf_predict(μ,Σ,u,kf_sys) 16 | 17 | # get probem data from kf_sys 18 | A,B,Q = kf_sys.A, kf_sys.B, kf_sys.Q 19 | 20 | # predict one step 21 | μ̄ = A*μ + B*u 22 | Σ̄ = A*Σ*A' + Q 23 | 24 | return μ̄, Σ̄ 25 | end 26 | 27 | function kf_innovate(μ̄,Σ̄,y,kf_sys) 28 | 29 | # get probem data from kf_sys 30 | A,C,R = kf_sys.A, kf_sys.C, kf_sys.R 31 | 32 | # innovation 33 | z = y - C*μ̄ 34 | S = C*Σ̄*C' + R 35 | 36 | # kalman gain 37 | L = (Σ̄*C')/S 38 | 39 | return z, L 40 | end 41 | function kf_update(μ̄,Σ̄,z,L,kf_sys) 42 | 43 | # problem data 44 | R,C = kf_sys.R,kf_sys.C 45 | 46 | # update (joseph form for Σ) 47 | μ₊= μ̄ + L*z 48 | Σ₊= (I - L*C)*Σ̄*(I - L*C)' + L*R*L' 49 | 50 | return μ₊, Σ₊ 51 | end 52 | --------------------------------------------------------------------------------