Merge branch 'measurement_model' of github.com:baggepinnen/LowLevelPa…

…rticleFilters.jl into measurement_model

docs/src/index.md

@@ -323,3 +323,32 @@ See examples in [Parameter Estimation](@ref).
 ## Tuning through simulation
 It is possible to sample from the Bayesian model implied by a filter and its parameters by calling the function [`simulate`](@ref). A simple tuning strategy is to adjust the noise parameters such that a simulation looks "similar" to the data, i.e., the data must not be too unlikely under the model.
 
+# Videos
+Several video tutorials using this package are available in the playlists
+- [System identification in Julia](https://www.youtube.com/playlist?list=PLC0QOsNQS8ha6SwaNOZDXyG9Bj8MPbF-q)
+- [Control systems in Julia](https://www.youtube.com/playlist?list=PLC0QOsNQS8hZtOQPHdtul3kpQwMOBL8Qc)
+
+Some examples featuring this package in particular are
+
+---
+
+**Using an optimizer to optimize the likelihood of an [`UnscentedKalmanFilter`](@ref):**
+```@raw html
+<iframe style="height: 315px; width: 560px" src="https://www.youtube.com/embed/0RxQwepVsoM" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
+```
+
+---
+
+**Estimation of time-varying parameters:**
+```@raw html
+<iframe style="height: 315px; width: 560px" src="https://www.youtube.com/embed/zJcOPPLqv4A?si=XCvpo3WD-4U3PJ2S" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
+```
+
+---
+
+**Adaptive control by means of estimation of time-varying parameters:**
+```@raw html
+<iframe style="height: 315px; width: 560px" src="https://www.youtube.com/embed/Ip_prmA7QTU?si=Fat_srMTQw5JtW2d" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
+```
+
+

docs/src/parameter_estimation.md

@@ -458,4 +458,9 @@ Estimation of time-varying parameters:
 <iframe style="height: 315px; width: 560px" src="https://www.youtube.com/embed/zJcOPPLqv4A?si=XCvpo3WD-4U3PJ2S" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
 ```
 
+Adaptive control by means of estimation of time-varying parameters:
+```@raw html
+<iframe style="height: 315px; width: 560px" src="https://www.youtube.com/embed/Ip_prmA7QTU?si=Fat_srMTQw5JtW2d" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
+```
+
 

src/ekf.jl

@@ -170,9 +170,18 @@ function smooth(sol, kf::AbstractExtendedKalmanFilter, u::AbstractVector=sol.u,
     RT[end]      = Rt[end]      |> copy
     for t = T-1:-1:1
         A = kf.Ajac(xT[t+1],u[t+1],p,((t+1)-1)*kf.Ts)
-        C     = Rt[t]*A'/R[t+1]
+        C     = Rt[t]*A'/cholesky(Symmetric(R[t+1]))
         xT[t] = C*(xT[t+1] .- x[t+1]) .+= xt[t]
-        RT[t] = symmetrize(C*(RT[t+1] .- R[t+1])*C') .+= Rt[t]
+        RD = RT[t+1] .- R[t+1]
+        RDC = RD*C'
+        if RD isa SMatrix
+            R0 = symmetrize(C*RDC)
+        else
+            CRDC = RD # Just a rename
+            mul!(CRDC, C, RDC)
+            R0 = symmetrize(CRDC)
+        end
+        RT[t] = @bangbang R0 .+= Rt[t]
     end
     xT,RT,ll
 end

src/smoothing.jl

@@ -1,19 +1,20 @@
 
 """
+    xT,RT,ll = smooth(sol, kf)
     xT,RT,ll = smooth(kf::KalmanFilter, u::Vector, y::Vector, p=parameters(kf))
     xT,RT,ll = smooth(kf::ExtendedKalmanFilter, u::Vector, y::Vector, p=parameters(kf))
 
-Returns smoothed estimates of state `x` and covariance `R` given all input output data `u,y`
+Returns smoothed estimates of state `x` and covariance `R` given all input output data `u,y` or an existing solution `sol` obtained from [`forward_trajectory`](@ref).
 """
-function smooth(sol::KalmanFilteringSolution, kf::KalmanFilter, u::AbstractVector, y::AbstractVector, p=parameters(kf))
+function smooth(sol::KalmanFilteringSolution, kf::KalmanFilter, u::AbstractVector=sol.u, y::AbstractVector=sol.y,  p=parameters(kf))
     (; x,xt,R,Rt,ll) = sol
     T            = length(y)
     xT           = similar(xt)
     RT           = similar(Rt)
     xT[end]      = xt[end]      |> copy
     RT[end]      = Rt[end]      |> copy
     for t = T-1:-1:1
-        C     = Rt[t]*get_mat(kf.A, xT[t+1], u[t+1], p, (t+1-1)*kf.Ts)'/R[t+1]
+        C     = Rt[t]*get_mat(kf.A, xT[t+1], u[t+1], p, (t+1-1)*kf.Ts)'/cholesky(Symmetric(R[t+1]))
         xT[t] = xt[t] .+ C*(xT[t+1] .- x[t+1])
         RT[t] = Rt[t] .+ symmetrize(C*(RT[t+1] .- R[t+1])*C')
     end

test/test_large.jl

@@ -105,6 +105,22 @@ plot(sol_ukf, plothy = true, plote = true, plotR=true)
 plot(sol_ekf, plothy = true, plote = true, plotRt=true)
 plot(sol_sqkf, plothy = true, plote = true)
 
+## Smoothing ===================================================================
+
+xT,RT,ll = smooth(sol_kf, kf)
+xT2,RT2,ll2 = smooth(sol_ekf, ekf)
+xT3,RT3,ll3 = smooth(sol_ukf, ukf)
+xT4,RT4,ll4 = smooth(sol_sqkf, ukf)
+
+@test xT2 ≈ xT
+@test RT2 ≈ RT
+
+@test xT3 ≈ xT
+@test RT3 ≈ RT
+
+@test xT4 ≈ xT
+@test RT4 ≈ RT
+
 ## Test mixing of measurement models ===========================================
 
 mm_ukf = UKFMeasurementModel{Float64, true, false}(measurement_large_ip, R2; nx, ny)
@@ -123,4 +139,4 @@ for mm in mms
 
     @test kf.x ≈ ekf.x ≈ ukf.x
     @test kf.R ≈ ekf.R ≈ ukf.R
-end
+end