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) 

using Makie

n, m = 500, 500 # no of pixels

# test location
i0 = 100
j0 = 150

# Euclidean distance between (i,j) and (i0, j0)
d((i,j), (i0, j0)) = sqrt((i - i0)^2 + (j - j0)^2)

# size of the "beetle"
a = 10^2

# make a picture (matrix) with a beetle (just a blob)
beetleimg(i0, j0) = [ exp(-d((i,j), (i0, j0))^2/(2*a))    for i in 1:m, j in 1:m]

# show it
image(beetleimg(10, 100))


# Normalize a picture to [0, 1]
nlz(img) = (img .- minimum(img))/(maximum(img) - minimum(img))



σ = 0.1 # noise on the pictures
# make pictures out of positions
imgs = [nlz(σ*randn(m, n) + beetleimg(pos[1], pos[2])) for pos in s]
image(imgs[1])

# find the coordinates of the maximum pixel
myfindmax(img) = convert(Tuple, findmax(img)[2])
ys = [myfindmax(img) for img in imgs]

# estimate the "error"
σest = mean(d.(ys, s)) # average error in image recognition

using Images
using FileIO
using VideoIO
using Makie
f = VideoIO.openvideo("ant.mov")
img = read(f)
imgs = Any[img]
while !eof(f)
    read!(f, img)
    push!(imgs, copy(img))
end
close(f)

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