├── .gitignore ├── README.md ├── bayesian_kalmanfilter.py ├── bayesian_kalmanfilter_test.py ├── kalmanfilter.py └── kalmanfilter_test.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by http://www.gitignore.io 2 | 3 | ### Python ### 4 | # Byte-compiled / optimized / DLL files 5 | __pycache__/ 6 | *.py[cod] 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | env/ 14 | bin/ 15 | build/ 16 | develop-eggs/ 17 | dist/ 18 | eggs/ 19 | lib/ 20 | lib64/ 21 | parts/ 22 | sdist/ 23 | var/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | 28 | # Installer logs 29 | pip-log.txt 30 | pip-delete-this-directory.txt 31 | 32 | # Unit test / coverage reports 33 | htmlcov/ 34 | .tox/ 35 | .coverage 36 | .cache 37 | nosetests.xml 38 | coverage.xml 39 | 40 | # Translations 41 | *.mo 42 | 43 | # Mr Developer 44 | .mr.developer.cfg 45 | .project 46 | .pydevproject 47 | 48 | # Rope 49 | .ropeproject 50 | 51 | # Django stuff: 52 | *.log 53 | *.pot 54 | 55 | # Sphinx documentation 56 | docs/_build/ 57 | 58 | 59 | 60 | ### Emacs ### 61 | # -*- mode: gitignore; -*- 62 | *~ 63 | \#*\# 64 | /.emacs.desktop 65 | /.emacs.desktop.lock 66 | *.elc 67 | auto-save-list 68 | tramp 69 | .\#* 70 | 71 | # Org-mode 72 | .org-id-locations 73 | *_archive 74 | 75 | # flymake-mode 76 | *_flymake.* 77 | 78 | # eshell files 79 | /eshell/history 80 | /eshell/lastdir 81 | 82 | # elpa packages 83 | /elpa/ 84 | 85 | 86 | ### vim ### 87 | [._]*.s[a-w][a-z] 88 | [._]s[a-w][a-z] 89 | *.un~ 90 | Session.vim 91 | .netrwhist 92 | *~ 93 | 94 | *.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Variational Bayesian Kalman Filter 2 | --------------------------------------- 3 | 4 | [Särkkä, S., & Nummenmaa, A. (2009). Recursive noise adaptive Kalman filtering by variational Bayesian approximations.IEEE Transactions on Automatic Control, 54(3).](http://www.lce.hut.fi/~ssarkka/pub/vb-akf-ieee.pdf) 5 | 6 | Pythonで実装したもの。通常のカルマンフィルタでは不可能な、時間変化する観測信号のノイズの分散を推定出来ます 7 | 8 | だた個人的な感覚では、分散の推定精度はそんなに良くはなく（真の分散値に至るまでけっこうな数のイテレーションを要する、ばらつきも大きい）、しかもヘタすると発散する… 9 | 10 | 普通のカルマンフィルタも一緒に入ってる 11 | 12 | ## メモ 13 | 14 | * 論文の通りにやったら上手く行かなかったので一部更新式に修正が入ってる 15 | * なぜ上手く行かないのかったのかはわかってない 16 | * 論文著者のページでmatlabコードは公開されている（2013年くらいに確認したときは少なくとも） 17 | 18 | ミスがあったら教えて下さい… 19 | -------------------------------------------------------------------------------- /bayesian_kalmanfilter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # coding: utf-8 3 | 4 | import numpy as np 5 | import scipy as sp 6 | from pylab import * 7 | 8 | # A variational bayesian kalman filter algorithm based on 9 | # "Recursive Noise Adaptive Kalman Filtering 10 | # by Variational Bayesian Approximations", 11 | # http://www.lce.hut.fi/~ssarkka/pub/vb-akf-ieee.pdf 12 | 13 | # state: x = A*x_ + B*u + w, w ~ N(0,Q) 14 | # observation: Y = H*x + v, v ~ N(0,Sigma) 15 | # Sigma ~ Inv-Gamma(alpha,beta) 16 | # Assuming Sigma is a diagonal matrix. 17 | class BayesianKalmanFilter: 18 | def __init__(self, A, B, u, Q, H, alpha, beta, rho, mu, Sigma, N=2): 19 | self.__init_lssm(A, B, u, Q, H) 20 | self.__init_hyper_parameters(alpha, beta) 21 | self.__init_state(mu, Sigma) 22 | # heuristic parameter 23 | self.rho = rho 24 | # the number of variational iterations 25 | self.N = N 26 | 27 | def __init_lssm(self, A, B, u, Q, H): 28 | # parameters for linear state space model (LSSM) 29 | self.A = A 30 | self.B = B 31 | self.u = u 32 | self.Q = Q 33 | self.H = H 34 | 35 | def __init_hyper_parameters(self, alpha, beta): 36 | # hyper parameters of Inv-Gamma 37 | self.alpha = alpha 38 | self.beta = beta 39 | 40 | def __init_state(self, mu, Sigma): 41 | # parameters of Gaussian 42 | self.mu = mu 43 | self.Sigma = Sigma 44 | 45 | def update(self, y): 46 | # 1. prediction 47 | mu_ = self.A * self.mu + self.B * self.u 48 | Sigma_ = self.Q + self.A * self.Sigma * self.A.T 49 | alpha_ = np.multiply(self.alpha, self.rho) 50 | beta_ = np.multiply(self.beta, self.rho) 51 | 52 | # 2. variational posteriori update 53 | # 論文に書いては以下のようにあるけど上手く行かなかったので変更 54 | #alpha_k = alpha_ + 0.5 55 | alpha_k = alpha_ 56 | beta_k = beta_ 57 | for iter in range(self.N): 58 | # update for N(mu,Sigma) 59 | tmp = np.squeeze(np.asarray(np.divide(beta_k, alpha_k))) 60 | Sigma_k = np.mat(np.diag(tmp)) 61 | yi = y - self.H * mu_ 62 | S = self.H * Sigma_ * self.H.T + Sigma_k 63 | K = Sigma_ * self.H.T * S.I 64 | self.mu = mu_ + K * yi 65 | self.Sigma = Sigma_ - K * self.H * Sigma_ 66 | 67 | # update for Inv-Gamma(alpha,beta) 68 | yii = y - self.H * self.mu 69 | SS = self.H * self.Sigma * self.H.T 70 | # alpha, betaともにiter毎に更新するように変更 71 | alpha_k = alpha_k + 0.5 72 | for i in range(len(self.beta)): 73 | # 論文の実装 74 | #beta_k[i] = beta_[i] + 0.5 * yii[i] * yii[i] + 0.5 * SS[i,i] 75 | beta_k[i] = beta_k[i] + 0.5 * yii[i] * yii[i] + 0.5 * SS[i,i] 76 | self.alpha = alpha_k 77 | self.beta = beta_k 78 | 79 | return self.mu,self.Sigma,np.divide(self.beta, self.alpha) 80 | -------------------------------------------------------------------------------- /bayesian_kalmanfilter_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # coding: utf-8 3 | 4 | import numpy as np 5 | import scipy as sp 6 | from pylab import * 7 | import bayesian_kalmanfilter as bkf 8 | 9 | if __name__ == '__main__': 10 | # 初期化 11 | T = 40000 # 観測数 12 | N = 2 # the number of variational iterations 13 | x = np.mat([[0],[0]]) # 初期位置 14 | X = [x] # 状態 15 | Y = [np.mat([[0],[0]])] # 観測 16 | 17 | # state x = A * x_ + B * u + w, w~N(0,Q) 18 | A = np.mat([[1,0],[0,1]]) 19 | B = np.mat([[1,0],[0,1]]) 20 | u = np.mat([[1],[1]]) 21 | Q = np.mat([[1,0],[0,1]]) 22 | 23 | # observation Y = C * x + v, v~N(0,R) 24 | C = np.mat([[1,0],[0,1]]) 25 | R = np.mat([[10,0],[0,10]]) 26 | 27 | # hyper parameters 28 | alpha = np.mat([[1],[1]]) 29 | beta = np.mat([[1],[1]]) 30 | rho = np.mat([[1-exp(-4)],[1-exp(-4)]]) 31 | 32 | # 観測データの生成 33 | for i in range(T): 34 | if i >= T/4: 35 | R = np.mat([[0.2,0],[0,0.2]]) 36 | if i >= 3*T/4: 37 | R = np.mat([[5.0,0],[0,5.0]]) 38 | x = A * x + B * u + np.random.multivariate_normal([0,0],Q,1).T 39 | X.append(x) 40 | y = C * x + np.random.multivariate_normal([0,0],R,1).T 41 | Y.append(y) 42 | 43 | # initial value 44 | mu = np.mat([[0],[0]]) 45 | Sigma = np.mat([[0.0,0.0],[0.0,0.0]]) 46 | M = [mu] # 状態推定の結果 47 | V = [np.divide(beta, alpha)] # 分散推定の結果 48 | 49 | # bayesian variational kalman filter 50 | bkf = bkf.BayesianKalmanFilter(A,B,u,Q,C,alpha,beta,rho,mu,Sigma,N) 51 | for i in range(T): 52 | mu,Sigma,obs_var = bkf.update(Y[i+1]) 53 | M.append(mu) 54 | V.append(obs_var) 55 | print i,float(obs_var[0]) 56 | 57 | # 描画 58 | subplot(311) 59 | a,b = np.array(np.concatenate(X,axis=1)) 60 | plot(a,b,'r-') 61 | a,b = np.array(np.concatenate(M,axis=1)) 62 | plot(a,b,'b--') 63 | a = np.array(np.concatenate(V,axis=1)) 64 | subplot(312) 65 | plot(a[0], 'b-') 66 | subplot(313) 67 | plot(a[1], 'b-') 68 | 69 | show() 70 | 71 | -------------------------------------------------------------------------------- /kalmanfilter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # coding: utf-8 3 | 4 | import numpy as np 5 | import scipy as sp 6 | from pylab import * 7 | 8 | class KalmanFilter: 9 | def __init__(self, A, B, u, Q, H, R, mu, Sigma): 10 | # parameters for linear state space model 11 | # state: x = A*x_ + B*u + w, w ~ N(0,Q) 12 | # observation: Y = H*x + v, v ~ N(0,R) 13 | self.A = A 14 | self.B = B 15 | self.u = u 16 | self.Q = Q 17 | self.H = H 18 | self.R = R 19 | # initialize 20 | self.mu = mu 21 | self.Sigma = Sigma 22 | 23 | def update(self, y): 24 | # 1. prediction step 25 | mu_ = self.A * self.mu + self.B * self.u 26 | Sigma_ = self.Q + self.A * self.Sigma * self.A.T 27 | # 2. correction step 28 | error = y - self.H * mu_ 29 | S = self.H * Sigma_ * self.H.T + self.R 30 | K = Sigma_ * self.H.T * S.I 31 | self.mu = mu_ + K * error 32 | self.Sigma = Sigma_ - K * self.H * Sigma_ 33 | 34 | return self.mu,self.Sigma 35 | -------------------------------------------------------------------------------- /kalmanfilter_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # coding: utf-8 3 | 4 | import numpy as np 5 | import scipy as sp 6 | from pylab import * 7 | import kalmanfilter as kf 8 | 9 | if __name__ == '__main__': 10 | # 初期化 11 | T = 30 # 観測数 12 | x = np.mat([[0],[0]]) # 初期位置 13 | X = [np.mat([[0],[0]])] # 状態 14 | Y = [np.mat([[0],[0],[0]])] # 観測 15 | 16 | # state x = A * x_ + B * u + w, w~N(0,Q) 17 | A = np.mat([[1,0],[0,1]]) 18 | B = np.mat([[1,0],[0,1]]) 19 | u = np.mat([[2],[2]]) 20 | Q = np.mat([[1,0],[0,1]]) 21 | 22 | # observation Y = C * x + v, v~N(0,R) 23 | C = np.mat([[1,0],[0,1]]) 24 | R = np.mat([[2,0],[0,2]]) 25 | 26 | # 観測データの生成 27 | for i in range(T): 28 | x = A * x + B * u + np.random.multivariate_normal([0,0],Q,1).T 29 | X.append(x) 30 | y = C * x + np.random.multivariate_normal([0,0],R,1).T 31 | Y.append(y) 32 | 33 | # initialize 34 | mu = np.mat([[0],[0]]) 35 | Sigma = np.mat([[0,0],[0,0]]) 36 | M = [mu] # 推定 37 | 38 | # Kalman filter 39 | kf = kf.KalmanFilter(A, B, u, Q, C, R, mu, Sigma) 40 | for i in range(T): 41 | mu,Sigma = kf.update(Y[i+1]) 42 | M.append(mu) 43 | 44 | # 描画 45 | a,b = np.array(np.concatenate(X,axis=1)) 46 | plt.plot(a,b,'rs-') 47 | a,b = np.array(np.concatenate(M,axis=1)) 48 | plt.plot(a,b,'bo-') 49 | #plt.axis('equal') 50 | xlabel("sample") 51 | plt.show() 52 | --------------------------------------------------------------------------------