kalmna filter

gistfile1.txt

using Distributions, Statistics, LinearAlgebra

## simulate sample trajectory

μ = [1.0, 0.0]
Σ = [0.5  0.0
     0.0  0.5]
x0 = rand(MultivariateNormal(μ, Σ))   #start point

β = [0.0, 0.0]
Q = [0.1  0.0
     0.0  0.1]

T = 1:10

# bulid trajectory
function sample_trajectory(x, T, β, Q)
    s = [x]
    for t in T
        x = x + rand(MultivariateNormal(β, Q))
        push!(s, x)
    end
    return s
end

s = sample_trajectory(x0, T, β, Q)

using Makie
lines(first.(s), last.(s))   #real trajectory

# add some noise

function noise(s, β, Q)
    s0 = [copy(m) for m in s]
    for t in 1:11
        s0[t] = s0[t] + rand(MultivariateNormal(β, Q))
    end
    return(s0)
end

noised_s = noise(s, β, Q)
scatter!(first.(noised_s), last.(noised_s))  #observed trajectory

F = Matrix{Float64}(I, 2, 2)
P = I
function predict(x, F, P, Q)
    x = F*x
    P = F*P*F' + Q
    x, P
end

# H: observation matrix
# F: state-trasition
# Q: the covariance of the process noise
# R: the covariance of the observation noise

H = Matrix{Float64}(I, 2, 2)
R = Q

function correct(x, y, Ppred, R, H)
    yres = y - H*x # innovation residual

    S = (H*Ppred*H' + R) # innovation covariance

    K = Ppred*H'/S # Kalman gain
    x = x + K*yres
    P = (I - K*H)*Ppred*(I - K*H)' + K*R*K' #  Joseph form

    x, P, yres, S
end

path = [x0]
x = predict(noised_s[1], F, P, Q)[1]
for i in 1:11
    pre = predict(x, F, P, Q)
    x = pre[1]
    P = pre[2]
    filter_s = correct(x, noised_s[i], P, R, H)
    x = filter_s[1]
    P = filter_s[2]
    push!(path, x)
end
lines!(first.(path), last.(path), linestyle = :dash) 

Working:

using Images
using FileIO
using VideoIO
using Makie
using Colors
#f = VideoIO.openvideo("ant.mov")
f = VideoIO.openvideo("beetle.mp4")
img = read(f)
imgs = [rotr90(Matrix(Gray.(img)))]
i = 1
while !eof(f) && i < 100
    global i += 1
    read!(f, img)
    push!(imgs, rotr90(Matrix(Gray.(img))))
end
close(f)

imgs1 = imgs


using AbstractPlotting
using AbstractPlotting.MakieLayout
nlz(x, (a,b) = extrema(x)) = (x .- a)./(b-a)

scene, layout = layoutscene(resolution = (1200, 900))
imgs2 = [img.*dim for (img,dim) in zip(imgs, diff(nlz.(imgs)))]
imgs = imgs1
ax = layout[1, 1] = LAxis(scene)
sl1 = layout[2, 1] = LSlider(scene, range = eachindex(imgs), startvalue = 1)

curimg = lift(i -> imgs[i], sl1.value)
image!(ax, curimg)
scene