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) 

kalmanandtrack.jl

using Images
using ImageTransformations
using FileIO
using VideoIO
using Makie
using Colors
using StaticArrays

#f = VideoIO.openvideo("ant.mov")
`ffmpeg -i beetle.mp4 -vf vidstabdetect -f null -`
#run(`ffmpeg -i beetle.mp4 -vf vidstabtransform,unsharp=5:5:0.8:3:3:0.4 beetle_stabilized.mp4`)

f = VideoIO.openvideo("beetle.mp4")
img = read(f)
imgs = [rotr90(Matrix(Gray.(restrict(img))))]
i = 1
while !eof(f) && i < 100
    global i += 1
    read!(f, img)
    push!(imgs, rotr90(Matrix(Gray.(restrict(img)))))
end
close(f)
scenesize = (960, 540+20)
imgs1 = imgs
n = length(imgs1)

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

using ImageFiltering
imgs2 = [imfilter(img, ImageFiltering.Kernel.gaussian(3)) for img in imgs1]

#=
using StaticArrays
imgs = imgs1
r = 3
n = length(imgs)
ss = [SVector(0, 0)]
stp = 20
for k in 1:stp:n-stp
    ra = CartesianIndices((-r:r, -r:r))
    A = [ssd(circshift(imgs[k+stp], (u[1], u[2]))[r:end-r, r:end-r], imgs[k][r:end-r, r:end-r]) for u in ra]
    s = findmin(A)[2]
    push!(ss, SVector(s[1] - r - 1, s[2] - r - 1))
end

cr = repeat(cumsum(ss), inner=stp)
newimgs = similar(imgs, length(cr))
for (k,u) in enumerate(cr)
    newimgs[k] = circshift(imgs[k], (u[1], u[2]))
end
scene, layout = layoutscene(resolution = scenesize)
#imgs2 = [dim for (img,dim) in zip(imgs, diff(imgs))]
imgs = imgs2
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
=#


#
using Distributions, Statistics, LinearAlgebra


function predict(x, F, P, Q)
    x = F*x
    P = F*P*F' + Q
    x, P
end


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


"""
     track(A::Matrix{Float64}, p, h = 10) -> p2, err

 Track blurry lightsource by applying a window with half-width `h` at
 an approximate location `p = (i,j)` and find the
 average weighted location of points with *high* light intensity.

 Gives an standard error estimate.
 """
 function track(img::Matrix, p, h = 10)
     i, j = round.(Int, p)
     CR = CartesianIndices((i-h:i+h, j-h:j+h))
     μ = mean(img[ci] for ci in CR)
     C = sum(max(img[ci] - μ, 0) for ci in CR)

     xhat = sum(max(img[ci] - μ, 0)*ci[1] for ci in CR)/C
     yhat = sum(max(img[ci] - μ, 0)*ci[2] for ci in CR)/C

     xerr =  sum(max(img[ci] - μ, 0)*(ci[1] - xhat)^2 for ci in CR)/C
     yerr =  sum(max(img[ci] - μ, 0)*(ci[2] - yhat)^2 for ci in CR)/C

     err = sqrt(xerr + yerr)
     SVector(xhat, yhat), err
 end



I2 = @SMatrix [1.0  0.0
      0.0  1.0]
P0 = I2
x0 = SVector(1536.0/2, 620.0/2)  #start point


Q0 = 1*I2

F = I2
T = 1:n
H = I2
R = I2

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


imgs = imgs2
path = [x0]
res = similar(path, 0)
let x = x0
    P = P0
    for i in 1:n
        x, P = predict(x, F, P, Q0)
        xobs, err = track(-imgs[i], x, 5)

        x, P, yres = correct(x, xobs, P, R, H)
        push!(path, x)
        push!(res, yres)
    end
end

lines(path)



I4 = SMatrix{4,4}(1.0I)
Z2 = 0I2
P0 = I4
x0 = SVector(1537.0/2, 620.0/2, 0.0, 0.0)  #start point


Q0 = SMatrix{4,4}([0.01I2 Z2; Z2 0.1I2])

F = @SMatrix [
    1.0 0.0 1.0 0.0
    0.0 1.0 0.0 1.0
    0.0 0.0 1.0 0.0
    0.0 0.0 0.0 1.0
]


T = 1:n
H = @SMatrix [
    1.0 0.0 0.0 0.0
    0.0 1.0 0.0 0.0
]
R = I2

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


imgs = imgs2
latent = [x0]
res = Any[]
let x = x0
    P = P0
    for i in 1:n
        x, P = predict(x, F, P, Q0)
        xobs, err = track(-imgs[i], x, 5)

        x, P, yres = correct(x, xobs, P, R, H)
        push!(latent, x)
        push!(res, yres)
    end
end
path = map(x->SVector(x[1], x[2]), latent)
vel = map(x->SVector(x[3], x[4]), latent)


image(imgs[1])
lines!(path)
arrows!(getindex.(latent, 1), getindex.(latent, 2), getindex.(latent, 3), getindex.(latent, 4))
lines!(10vel .+ Ref(mean(path)))

scene, layout = layoutscene(resolution = scenesize)
imgs = imgs1
ax = layout[1, 1] = LAxis(scene)
sl1 = layout[2, 1] = LSlider(scene, range = eachindex(imgs), startvalue = 3)
curimg = lift(i -> imgs[i], sl1.value)
image!(ax, curimg)
scene

curpos = lift(i -> [path[i]], sl1.value)
#scatter!(ax, curpos, markersize=10, strokecolor=:red, strokewidth=2, color= RGBA(0.2, 1.0, 0.0, 0.0))

curs = [lift(i -> [ latent[i][k]], sl1.value) for k in 1:4]
arrows!(ax, curs..., arrowcolor=:red, linecolor=:red, arrowsize=3, linewidth=2, lengthscale=10)

scene