<s> """<STR_LIT>""" <EOL> from __future__ import ( absolute_import , division , print_function , <EOL> unicode_literals ) <EOL> import numpy as np <EOL> from scipy . linalg import inv <EOL> from numpy import dot , zeros , eye , outer <EOL> from numpy . random import multivariate_normal <EOL> from filterpy . common import dot3 <EOL> class EnsembleKalmanFilter ( object ) : <EOL> """<STR_LIT>""" <EOL> def __init__ ( self , x , P , dim_z , dt , N , hx , fx ) : <EOL> """<STR_LIT>""" <EOL> assert dim_z > <NUM_LIT:0> <EOL> self . dim_x = len ( x ) <EOL> self . dim_z = dim_z <EOL> self . dt = dt <EOL> self . N = N <EOL> self . hx = hx <EOL> self . fx = fx <EOL> self . Q = eye ( self . dim_x ) <EOL> self . R = eye ( self . dim_z ) <EOL> self . mean = [ <NUM_LIT:0> ] * self . dim_x <EOL> self . initialize ( x , P ) <EOL> def initialize ( self , x , P ) : <EOL> """<STR_LIT>""" <EOL> assert x . ndim == <NUM_LIT:1> <EOL> self . sigmas = multivariate_normal ( mean = x , cov = P , size = self . N ) <EOL> self . x = x <EOL> self . P = P <EOL> def update ( self , z , R = None ) : <EOL> """<STR_LIT>""" <EOL> if z is None : <EOL> return <EOL> if R is None : <EOL> R = self . R <EOL> if np . isscalar ( R ) : <EOL> R = eye ( self . dim_z ) * R <EOL> N = self . N <EOL> dim_z = len ( z ) <EOL> sigmas_h = zeros ( ( N , dim_z ) ) <EOL> for i in range ( N ) : <EOL> sigmas_h [ i ] = self . hx ( self . sigmas [ i ] ) <EOL> z_mean = np . mean ( sigmas_h , axis = <NUM_LIT:0> ) <EOL> P_zz = <NUM_LIT:0> <EOL> for sigma in sigmas_h : <EOL> s = sigma - z_mean <EOL> P_zz += outer ( s , s ) <EOL> P_zz = P_zz / ( N - <NUM_LIT:1> ) + R <EOL> P_xz = <NUM_LIT:0> <EOL> for i in range ( N ) : <EOL> P_xz += outer ( self . sigmas [ i ] - self . x , sigmas_h [ i ] - z_mean ) <EOL> P_xz /= N - <NUM_LIT:1> <EOL> K = dot ( P_xz , inv ( P_zz ) ) <EOL> e_r = multivariate_normal ( [ <NUM_LIT:0> ] * dim_z , R , N ) <EOL> for i in range ( N ) : <EOL> self . sigmas [ i ] += dot ( K , z + e_r [ i ] - sigmas_h [ i ] ) <EOL> self . x = np . mean ( self . sigmas , axis = <NUM_LIT:0> ) <EOL> self . P = self . P - dot3 ( K , P_zz , K . T ) <EOL> def predict ( self ) : <EOL> """<STR_LIT>""" <EOL> N = self . N <EOL> for i , s in enumerate ( self . sigmas ) : <EOL> self . sigmas [ i ] = self . fx ( s , self . dt ) <EOL> e = multivariate_normal ( self . mean , self . Q , N ) <EOL> self . sigmas += e <EOL> P = <NUM_LIT:0> <EOL> for s in self . sigmas : <EOL> sx = s - self . x <EOL> P += outer ( sx , sx ) <EOL> self . P = P / ( N - <NUM_LIT:1> ) </s>
