LoginSignup
7
4

More than 3 years have passed since last update.

Juliaでカルマンフィルタを実装してみた

Posted at

参考資料

状態空間モデルとカルマンフィルタ

観測変数を$y_t$、状態変数を$\alpha_t$とする。
状態変数の値を逐次推定するカルマンフィルタのアルゴリズムは以下のようになる。

\begin{align}
y_t&=Z_t\alpha_t+\epsilon_t,\quad\epsilon_t\sim N(0,H_t)\\
\alpha_t&=T_t\alpha_{t-1}+\eta_t,\quad\eta_t\sim N(0,Q_t)\\
\alpha_0&\sim N(a_0,\Sigma_0)\\
a_{t|t-1}&=T_ta_{t-1|t-1}\quad\text{(forecasting)}\\
\Sigma_{t|t-1}&=T_t\Sigma_{t-1|t-1}T_t'+Q_t\\
y_{t|t-1}&=Z_ta_{t|t-1}\\
F_{t|t-1}&=Z_t\Sigma_{t|t-1}Z_t'+H_t\\
K_t&=\Sigma_{t|t-1}Z_t'F_{t|t-1}^{-1}\\
a_{t|t}&=a_{t|t-1}+K_t(y_t-y_{t|t-1})\quad\text{(filtering)}\\
\Sigma_{t|t}&=\Sigma_{t|t-1}-K_tF_{t|t-1}K_t'\\
C_{t-1}&=\Sigma_{t-1|t-1}T_t'\Sigma_{t|t-1}^{-1}\\
a_{t-1|T}&=a_{t-1|t-1}+C_{t-1}(a_{t|T}-a_{t|t-1})\quad\text{(smoothing)}\\
\Sigma_{t-1|T}&=\Sigma_{t-1|t-1}+C_{t-1}(\Sigma_{t|T}-\Sigma_{t|t-1})C_{t-1}'
\end{align}

状態空間モデルの対数尤度は以下のようになる。

\begin{align}
\Pr(y_T,\dots,y_1)&=\prod_{t=1}^T\Pr(y_t|\Omega_{t-1})\\
\Pr(y_t|\Omega_{t-1})&=N(y_t|y_{t|t-1},F_{t|t-1})\\
\log\Pr(y_T,\dots,y_1)&=-\frac{T}{2}\log(2\pi)-\frac{1}{2}\sum_{t=1}^T\log|F_{t|t-1}|
-\frac{1}{2}\sum_{t=1}^T(y_t-y_{t|t-1})'F_{t|t-1}^{-1}(y_t-y_{t|t-1})
\end{align}

$y_t$はスカラーとし、遷移行列$T_t$と誤差の分散$H_t,Q_t$は時刻$t$によらず一定とする。
状態変数の初期値$a_0,\Sigma_0$を与えて、対数尤度を最大化する$H_t,Q_t$を求める。

Juliaによる実装

状態変数の平均をa、分散をsigmaで表す。
forecastingを_a、filteringを_m、smoothingを_sで表す。
buildDLM()の引数dは、状態変数の次元数。

mutable struct MyDLM
  y::Array{Float64,1}
  a_m::Array{Float64,2}
  sigma_m::Array{Float64,3}
  y_m::Array{Float64,1}
  Z::Array{Float64,2}
  T::Array{Float64,2}
  H::Float64
  Q::Array{Float64,2}
  a_a::Array{Float64,2}
  sigma_a::Array{Float64,3}
  y_a::Array{Float64,1}
  F::Array{Float64,1}
  K::Array{Float64,2}
  a_s::Array{Float64,2}
  sigma_s::Array{Float64,3}
  y_s::Array{Float64,1}
  y_s_sigma::Array{Float64,1}
  C::Array{Float64,3}
  MyDLM() = new()
end

function buildDLM(y::Vector{<:Number},d::Integer)
  n = length(y)
  o = MyDLM()
  o.y = copy(y)
  o.a_m = zeros((d,n+1))
  o.sigma_m = zeros((d,d,n+1))
  o.y_m = zeros(n)
  o.Z = zeros(n,d)
  o.T = zeros(d,d)
  o.H = 0
  o.Q = zeros(d,d)
  o.a_a = zeros((d,n))
  o.sigma_a = zeros((d,d,n))
  o.y_a = zeros(n)
  o.F = zeros(n)
  o.K = zeros((d,n))
  o.a_s = zeros((d,n))
  o.sigma_s = zeros((d,d,n))
  o.y_s = zeros(n)
  o.y_s_sigma = zeros(n)
  o.C = zeros(d,d,n-1)
  o
end

function filter!(o::MyDLM,a0::Vector{<:Number},s0::Matrix{<:Number})
  n = length(o.y)
  o.a_m[:,1] = a0
  o.sigma_m[:,:,1] = s0
  for i in 1:n
    o.a_a[:,i] = o.T * o.a_m[:,i]
    o.sigma_a[:,:,i] = o.T * o.sigma_m[:,:,i] * o.T' + o.Q
    o.y_a[i] = dot(o.Z[i:i,:],o.a_a[:,i])
    o.F[i] = dot(o.Z[i:i,:] * o.sigma_a[:,:,i],o.Z[i:i,:]) + o.H
    o.K[:,i] = o.sigma_a[:,:,i] * o.Z[i:i,:]' / o.F[i]
    o.a_m[:,i+1] = o.a_a[:,i] + o.K[:,i] * (o.y[i] - o.y_a[i])
    o.sigma_m[:,:,i+1] = o.sigma_a[:,:,i] - o.K[:,i] * o.F[i] * o.K[:,i]'
    o.y_m[i] = dot(o.Z[i:i,:],o.a_m[:,i+1])
  end
end

function smooth!(o::MyDLM)
  n = length(o.y)
  o.a_s[:,n] = o.a_m[:,n+1]
  o.sigma_s[:,:,n] = o.sigma_m[:,:,n+1]
  o.y_s[n] = dot(o.Z[n:n,:],o.a_s[:,n])
  o.y_s_sigma[n] = dot(o.Z[n:n,:] * o.sigma_s[:,:,n], o.Z[n:n,:]) + o.H
  for i in (n-1):(-1):1
    o.C[:,:,i] = (o.sigma_m[:,:,i+1] * o.T') / o.sigma_a[:,:,i+1]
    o.a_s[:,i] = o.a_m[:,i+1] + o.C[:,:,i] * (o.a_s[:,i+1] - o.a_a[:,i+1])
    o.sigma_s[:,:,i] = o.sigma_m[:,:,i+1] + o.C[:,:,i] * (o.sigma_s[:,:,i+1] - o.sigma_a[:,:,i+1]) * o.C[:,:,i]'
    o.y_s[i] = dot(o.Z[i:i,:],o.a_s[:,i])
    o.y_s_sigma[i] = dot(o.Z[i:i,:] * o.sigma_s[:,:,i], o.Z[i:i,:]) + o.H
  end
end

# negative log likelihood
function nll(o::MyDLM)
  sum(log.(o.F) + (o.y-o.y_a) .^ 2 ./ o.F) / 2
end

使用するパッケージと補助関数

using CSV
using DataFrames
using StatsBase
using LinearAlgebra
using Optim
using Plots

eye(v::Number,d::Integer) = Matrix(I*v,d,d)

function my_diagm(m1::Matrix, m2::Matrix)
    ret = zeros(size(m1) .+ size(m2))
    ret[1:size(m1,1),1:size(m1,2)] = m1
    ret[(size(m1,1)+1):end,(size(m1,2)+1):end] = m2
    ret
end

ナイル川の年間水量の分析

1871年から1970年までの100年間に渡るナイル川の年間水量の変化をモデル化する。

# Nile 1871-1970
y = [1120,1160, 963,1210,1160,1160, 813,1230,1370,1140, 995, 935,
     1110, 994,1020, 960,1180, 799, 958,1140,1100,1210,1150,1250,
     1260,1220,1030,1100, 774, 840, 874, 694, 940, 833, 701, 916,
      692,1020,1050, 969, 831, 726, 456, 824, 702,1120,1100, 832,
      764, 821, 768, 845, 864, 862, 698, 845, 744, 796,1040, 759,
      781, 865, 845, 944, 984, 897, 822,1010, 771, 676, 649, 846,
      812, 742, 801,1040, 860, 874, 848, 890, 744, 749, 838,1050,
      918, 986, 797, 923, 975, 815,1020, 906, 901,1170, 912, 746,
      919, 718, 714, 740]

n = length(y)
a0 = mean(y[1:10])

ローカルレベルモデル

状態変数はレベルのみ。
レベルの初期値は先頭10年間の平均値、分散は$10^7$とする。

title = "local level"
model = buildDLM(y,1)
model.Z = ones(n,1)
model.T = ones(1,1)
function my_nll_optim(par)
  model.H = exp(par[1])
  model.Q = diagm(0 => [exp(par[2])])
  filter!(model,[a0],eye(1e7,1))
  nll(model)
end
result = optimize(my_nll_optim,zeros(2),NelderMead())
my_nll_optim(result.minimizer)
smooth!(model)
plot(y, label="true", fmt=:png, title=title)
plot!(model.y_a, label="forecast")
sd = sqrt.(model.y_s_sigma)
plot!(model.y_s, label="smoothed", ribbon=sd, fillalpha=.2)

local_level.jpg

ローカルトレンドモデル

状態変数はレベルとトレンド。レベルに対する誤差の分散はゼロとする。

title = "local trend"
model = buildDLM(y,2)
model.Z = hcat(ones(n),zeros(n))
model.T = [1 1;0 1]
function my_nll_optim(par)
  model.H = exp(par[1])
  model.Q = diagm(0 => [0,exp(par[2])])
  filter!(model,[a0,0],eye(1e7,2))
  nll(model)
end
result = optimize(my_nll_optim,zeros(2),NelderMead())

local_trend.jpg

回帰ローカルレベルモデル

Aswan Low Damの建設が1899年から始まっているので、
最初の28年間はゼロで、それ以降は1となる外部変数damを定義。
状態変数はレベルと回帰係数。

title = "regression"
dam = vcat(zeros(28),ones(n-28))
model = buildDLM(y,2)
model.Z = hcat(ones(n,1),dam)
model.T = [1 0;0 1]
function my_nll_optim(par)
  model.H = exp(par[1])
  model.Q = diagm(0 => exp.(par[2:3]))
  filter!(model,[a0,0],eye(1e7,2))
  nll(model)
end
result = optimize(my_nll_optim,zeros(3),NelderMead())

regression.jpg

回帰ローカルトレンドモデル

状態変数はレベルとトレンドと回帰係数。レベルに対する誤差の分散はゼロとする。

title = "regression + local trend"
dam = vcat(zeros(28),ones(n-28))
model = buildDLM(y,3)
model.Z = hcat(ones(n),zeros(n),dam)
model.T = [1 1 0;0 1 0;0 0 1]
function my_nll_optim(par)
  model.H = exp(par[1])
  model.Q = diagm(0 => vcat(0,exp.(par[2:3])))
  filter!(model,[a0,0,0],eye(1e7,3))
  nll(model)
end
result = optimize(my_nll_optim,zeros(3),NelderMead())

regression_trend.jpg

英国ドライバーの月次死傷者数の分析

英国ドライバーの月次死傷者数(1969/1-1984/12、対数値)の変化を
石油価格(対数値)と干渉変数(英国シートベルト法)に基づいてモデル化する。
干渉変数の値は、1983/1まではゼロで、それ以降は1とする。

df_uk = CSV.read("UKdriversKSI.txt",header=true,delim="\t")
y = log.(df_uk[:,1])
n = length(y)
df_price = CSV.read("logUKpetrolprice.txt",header=true,delim="\t")
price = df_price[:,1]
belt = vcat(zeros(169),ones(n-169))

回帰ローカルレベル季節性モデル

状態変数はレベルと回帰係数(シートベルト、石油価格)と季節指数(11個)。
誤差の分散がゼロでない季節指数は先頭の1個のみ。

title = "The local level model with seatbelt and logUKpetrolprice"
d = 14
dpar = 5
T_seasonal = diagm(-1 => ones(10))
T_seasonal[1,:] = -ones(11)
model = buildDLM(y,d)
model.Z = hcat(ones(n),belt,price,ones(n),zeros(n,10))
model.T = my_diagm(eye(1.0,3),T_seasonal)
function my_nll_optim(par)
  model.H = exp(par[1])
  model.Q = diagm(0 => vcat(exp.(par[2:dpar]),zeros(10)))
  filter!(model,zeros(d),eye(1e7,d))
  nll(model)
end
result = optimize(my_nll_optim,repeat([-1.0],dpar),NelderMead())
my_nll_optim(result.minimizer)
smooth!(model)
plot(y, label="true", title=title, fmt=:png, size=(800,400))
sd = sqrt.(model.y_s_sigma)
plot!(model.y_s, label="smoothed", ribbon=sd, fillalpha=.2)
level = model.a_s[1,:] + model.a_s[2,:] .* belt + model.a_s[3,:] .* price
plot!(level, label="level")

uk.jpg

状態変数の誤差の分散はほぼゼロとなっている。

exp.(result.minimizer)
[0.00401866, 2.2346e-9, 5.34704e-11, 5.15436e-5, 4.65412e-9]
7
4
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
7
4