def update(self, z, R=None, H=None):
# set to None to force recompute
self._log_likelihood = None
self._likelihood = None
self._mahalanobis = None
if np.all(np.isnan(z)):
x_mean_velocity = np.mean(self.velocity_x[-5:])
y_mean_velocity = np.mean(self.velocity_y[-5:])
z_noise = np.random.normal(loc=0, scale=0, size=2)
self.z = list(
self.x[0:2] - [self.velocity_x[-1], self.velocity_y[-1]] + [x_mean_velocity, y_mean_velocity] + z_noise)
z = list(
self.x[0:2] - [self.velocity_x[-1], self.velocity_y[-1]] + [x_mean_velocity, y_mean_velocity] + z_noise)
z = reshape_z(z, self.dim_z, self.x.ndim)
self.zs.append(z)
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
if H is None:
H = self.H
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, self.x)
# common subexpression for speed
PHT = dot(self.P, H.T)
# S = HPH' + R
# project system uncertainty into measurement space
self.S = dot(H, PHT) + R
self.SI = self.inv(self.S)
# K = PH'inv(S)
# map system uncertainty into kalman gain
self.K = dot(PHT, self.SI)
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = self.x + dot(self.K, self.y)
# P = (I-KH)P(I-KH)' + KRK'
# This is more numerically stable
# and works for non-optimal K vs the equation
# P = (I-KH)P usually seen in the literature.
I_KH = self._I - dot(self.K, H)
# self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T)
self.P = dot(I_KH, self.P)
# save measurement and posterior state
self.z = deepcopy(z)
self.x_post = self.x.copy()
self.P_post = self.P.copy()
def update(self, z, R_inv=None):
"""
Add a new measurement (z) to the kalman filter. If z is None, nothing
is changed.
Parameters
----------
z : np.array
measurement for this update.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
"""
if z is None:
return
if R_inv is None:
R_inv = self.R_inv
elif np.isscalar(R_inv):
R_inv = eye(self.dim_z) * R_inv
# rename for readability and a tiny extra bit of speed
H = self.H
H_T = H.T
P_inv = self.P_inv
x = self.x
if self._no_information:
self.x = dot(P_inv, x) + dot(H_T, R_inv).dot(z)
self.P_inv = P_inv + dot(H_T, R_inv).dot(H)
self.log_likelihood = math.log(sys.float_info.min)
self.likelihood = sys.float_info.min
else:
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, x)
# S = HPH' + R
# project system uncertainty into measurement space
self.S = P_inv + dot(H_T, R_inv).dot(H)
self.K = dot(self.inv(self.S), H_T).dot(R_inv)
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = x + dot(self.K, self.y)
self.P_inv = P_inv + dot(H_T, R_inv).dot(H)
self.z = np.copy(reshape_z(z, self.dim_z, np.ndim(self.x)))
if self.compute_log_likelihood:
self.log_likelihood = logpdf(x=self.y, cov=self.S)
self.likelihood = math.exp(self.log_likelihood)
if self.likelihood == 0:
self.likelihood = sys.float_info.min
def update(self, z, R=None, H=None):
"""与原update函数的唯一不同在于P更新的方式可以调节，为了保证其它功能正常，仍然保留原始的内容
"""
# set to None to force recompute
self._log_likelihood = None
self._likelihood = None
self._mahalanobis = None
if z is None:
self.z = np.array([[None] * self.dim_z]).T
self.x_post = self.x.copy()
self.P_post = self.P.copy()
self.y = np.zeros((self.dim_z, 1))
return
z = reshape_z(z, self.dim_z, self.x.ndim)
if R is None:
R = self.R
elif np.isscalar(R):
R = eye(self.dim_z) * R
if H is None:
H = self.H
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, self.x)
# common subexpression for speed
PHT = dot(self.P, H.T)
# S = HPH' + R
# project system uncertainty into measurement space
self.S = dot(H, PHT) + R
self.SI = self.inv(self.S)
# K = PH'inv(S)
# map system uncertainty into kalman gain
self.K = dot(PHT, self.SI)
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = self.x + dot(self.K, self.y)
# P = (I-KH)P(I-KH)' + KRK'
# This is more numerically stable
# and works for non-optimal K vs the equation
# P = (I-KH)P usually seen in the literature.
I_KH = self._I - dot(self.K, H)
# self.P = dot(I_KH, self.P) # 通常情况下的处理
self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T)
# save measurement and posterior state
self.z = deepcopy(z)
self.x_post = self.x.copy()
self.P_post = self.P.copy()
def update_steadystate(x, z, K, H=None):
"""
Add a new measurement (z) to the Kalman filter. If z is None, nothing
is changed.
Parameters
----------
x : numpy.array(dim_x, 1), or float
State estimate vector
z : (dim_z, 1): array_like
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be convertible to a column vector.
K : numpy.array, or float
Kalman gain matrix
H : numpy.array(dim_x, dim_x), or float, optional
Measurement function. If not provided, a value of 1 is assumed.
Returns
-------
x : numpy.array
Posterior state estimate vector
Examples
--------
This can handle either the multidimensional or unidimensional case. If
all parameters are floats instead of arrays the filter will still work,
and return floats for x, P as the result.
>>> update_steadystate(1, 2, 1) # univariate
>>> update_steadystate(x, P, z, H)
"""
if z is None:
return x
if H is None:
H = np.array([1])
if np.isscalar(H):
H = np.array([H])
Hx = np.atleast_1d(dot(H, x))
z = reshape_z(z, Hx.shape[0], x.ndim)
# error (residual) between measurement and prediction
y = z - Hx
# estimate new x with residual scaled by the kalman gain
return x + dot(K, y)
def __init__(self, dim_x, dim_z, dim_u=0):
self.dim_x = dim_x
self.dim_z = dim_z
self.dim_u = dim_u
# Set templates that will hook the assignments and cast to
# the specific shape
self.x = properties.MatrixTemplate(dim_x, 1)
self.P = properties.MatrixTemplate(dim_x, dim_x)
self.Q = properties.MatrixTemplate(dim_x, dim_x)
self.B = properties.MatrixTemplate(dim_x, dim_u)
self.F = properties.MatrixTemplate(dim_x, dim_x)
self.R = properties.MatrixTemplate(dim_z, dim_z)
self.z = properties.MatrixTemplate(dim_z, 1)
self.x = zeros((dim_x, 1)) # state
self.P = eye(dim_x) # uncertainty covariance
self.B = 0 # control transition matrix
self.F = np.eye(dim_x) # state transition matrix
self.R = eye(dim_z) # state uncertainty
self.Q = eye(dim_x) # process uncertainty
self.y = zeros((dim_z, 1)) # residual
z = np.array([None]*self.dim_z)
self.z = reshape_z(z, self.dim_z, self.x.ndim)
# gain and residual are computed during the innovation step. We
# save them so that in case you want to inspect them for various
# purposes
self.K = np.zeros(self.x.shape) # kalman gain
self.y = zeros((dim_z, 1))
self.S = np.zeros((dim_z, dim_z)) # system uncertainty
self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty
# identity matrix. Do not alter this.
self._I = np.eye(dim_x)
self._log_likelihood = log(sys.float_info.min)
self._likelihood = sys.float_info.min
self._mahalanobis = None
# these will always be a copy of x,P after predict() is called
self.x_prior = self.x.copy()
self.P_prior = self.P.copy()
# these will always be a copy of x,P after update() is called
self.x_post = self.x.copy()
self.P_post = self.P.copy()
def get_update(self, z=None):
"""
Computes the new estimate based on measurement `z` and returns it
without altering the state of the filter.
Parameters
----------
z : (dim_z, 1): array_like
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be convertible to a column vector.
Returns
-------
(x, P) : tuple
State vector and covariance array of the update.
"""
if z is None:
return self.x, self.P
z = reshape_z(z, self.dim_z, self.x.ndim)
R = self.R
H = self.H
P = self.P
x = self.x
# error (residual) between measurement and prediction
y = z - dot(H, x)
# common subexpression for speed
PHT = dot(P, H.T)
# project system uncertainty into measurement space
S = dot(H, PHT) + R
# map system uncertainty into kalman gain
K = dot(PHT, self.inv(S))
# predict new x with residual scaled by the kalman gain
x = x + dot(K, y)
# P = (I-KH)P(I-KH)' + KRK'
I_KH = self._I - dot(K, H)
P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
return x, P
def __init__(self, dim_x, dim_z, dim_u=0, compute_log_likelihood=True):
if dim_z < 1:
raise ValueError('dim_x must be 1 or greater')
if dim_z < 1:
raise ValueError('dim_x must be 1 or greater')
if dim_u < 0:
raise ValueError('dim_x must be 0 or greater')
self.dim_x = dim_x
self.dim_z = dim_z
self.dim_u = dim_u
self.x = zeros((dim_x, 1)) # state
self.P = eye(dim_x) # uncertainty covariance
self.Q = eye(dim_x) # process uncertainty
self.B = None # control transition matrix
self.F = eye(dim_x) # state transition matrix
self.H = zeros((dim_z, dim_x)) # Measurement function
self.R = eye(dim_z) # state uncertainty
self._alpha_sq = 1. # fading memory control
self.M = np.zeros(
(dim_z, dim_z)) # process-measurement cross correlation
self.z = reshape_z(zeros((dim_z)), self.dim_z, self.x.ndim)
# gain and residual are computed during the innovation step. We
# save them so that in case you want to inspect them for various
# purposes
self.K = np.zeros((dim_x, dim_z)) # kalman gain
self.y = zeros((dim_z, 1))
self.S = np.zeros((dim_z, dim_z)) # system uncertainty
# identity matrix. Do not alter this.
self._I = np.eye(dim_x)
# these will always be a copy of x,P after predict() is called
self.x_prior = np.copy(self.x)
self.P_prior = np.copy(self.P)
self.compute_log_likelihood = compute_log_likelihood
self.log_likelihood = math.log(sys.float_info.min)
self.likelihood = sys.float_info.min
self.inv = np.linalg.inv
def __init__(self, dim_x, dim_z, dim_u=0):
self.dim_x = dim_x
self.dim_z = dim_z
self.dim_u = dim_u
self.x = zeros((dim_x, 1)) # state
# uncertainty covariance
self.U = eye(dim_x)
self.D = ones((dim_x))
self.B = 0 # control transition matrix
self.F = np.eye(dim_x) # state transition matrix
# state uncertainty
self.Dm = eye(dim_z) #Decorrelation matrix
self.Ur = eye(dim_z) #Decorrelation matrix
self.Dr = ones((dim_z))
# process uncertainty
self.Uq = eye(dim_x)
self.Dq = ones((dim_x))
z = np.array([None]*self.dim_z)
self.z = reshape_z(z, self.dim_z, self.x.ndim)
# residual is computed during the innovation step. We
# save them so that in case you want to inspect it for various
# purposes
self.y = zeros((dim_z, 1)) # residual
self.S = np.zeros((dim_z, dim_z)) # system uncertainty
self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty
self._log_likelihood = log(sys.float_info.min)
self._likelihood = sys.float_info.min
self._mahalanobis = None
# these will always be a copy of x,P after predict() is called
self.x_prior = self.x.copy()
self.U_prior = self.U.copy()
self.D_prior = self.D.copy()
# these will always be a copy of x,P after update() is called
self.x_post = self.x.copy()
self.U_post = self.U.copy()
self.D_post = self.D.copy()
def __init__(self, dim_x, dim_z, dim_u=0):
self.dim_x = dim_x
self.dim_z = dim_z
self.dim_u = dim_u
self.x = zeros((dim_x, 1)) # state
self.P = eye(dim_x) # uncertainty covariance
self.B = 0 # control transition matrix
self.F = np.eye(dim_x) # state transition matrix
self.R = eye(dim_z) # state uncertainty
self.Q = eye(dim_x) # process uncertainty
self.y = zeros((dim_z, 1)) # residual
z = np.array([None]*self.dim_z)
self.z = reshape_z(z, self.dim_z, self.x.ndim)
# gain and residual are computed during the innovation step. We
# save them so that in case you want to inspect them for various
# purposes
self.K = np.zeros(self.x.shape) # kalman gain
self.y = zeros((dim_z, 1))
self.S = np.zeros((dim_z, dim_z)) # system uncertainty
self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty
# identity matrix. Do not alter this.
self._I = np.eye(dim_x)
self._log_likelihood = log(sys.float_info.min)
self._likelihood = sys.float_info.min
self._mahalanobis = None
# these will always be a copy of x,P after predict() is called
self.x_prior = self.x.copy()
self.P_prior = self.P.copy()
# these will always be a copy of x,P after update() is called
self.x_post = self.x.copy()
self.P_post = self.P.copy()
def find_likelihood(self, z, R=None, H=None):
if np.all(np.isnan(z)):
x_mean_velocity = np.mean(self.velocity_x[-5:])
y_mean_velocity = np.mean(self.velocity_y[-5:])
z_noise = np.random.normal(loc=0, scale=0, size=2)
z = list(
self.x[0:2] - [self.velocity_x[-1], self.velocity_y[-1]] + [x_mean_velocity, y_mean_velocity] + z_noise)
z = reshape_z(z, self.dim_z, self.x.ndim)
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
if H is None:
H = self.H
y = z - dot(H, self.x)
PHT = dot(self.P, H.T)
S = dot(H, PHT) + R
_log_likelihood = logpdf(x=y, cov=S)
_likelihood = exp(_log_likelihood)
if _likelihood == 0:
_likelihood = sys.float_info.min
return _likelihood
def update(self, z, R=None, H=None):
"""
Add a new measurement (z) to the Kalman filter.
If z is None, nothing is computed. However, x_post and P_post are
updated with the prior (x_prior, P_prior), and self.z is set to None.
Parameters
----------
z : (dim_z, 1): array_like
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be convertible to a column vector.
If you pass in a value of H, z must be a column vector the
of the correct size.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
H : np.array, or None
Optionally provide H to override the measurement function for this
one call, otherwise self.H will be used.
"""
# set to None to force recompute
self._log_likelihood = None
self._likelihood = None
self._mahalanobis = None
if z is None:
self.z = np.array([[None] * self.dim_z]).T
self.x_post = self.x.copy()
self.P_post = self.P.copy()
self.y = zeros((self.dim_z, 1))
return
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
if H is None:
z = reshape_z(z, self.dim_z, self.x.ndim)
H = self.H
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, self.x)
# common subexpression for speed
PHT = dot(self.P, H.T)
# S = HPH' + R
# project system uncertainty into measurement space
self.S = dot(H, PHT) + R
self.SI = self.inv(self.S)
# K = PH'inv(S)
# map system uncertainty into kalman gain
self.K = dot(PHT, self.SI)
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = self.x + dot(self.K, self.y)
# P = (I-KH)P(I-KH)' + KRK'
# This is more numerically stable
# and works for non-optimal K vs the equation
# P = (I-KH)P usually seen in the literature.
I_KH = self._I - dot(self.K, H)
self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T)
# save measurement and posterior state
self.z = deepcopy(z)
self.x_post = self.x.copy()
self.P_post = self.P.copy()
def update_correlated(self, z, R=None, H=None):
""" Add a new measurement (z) to the Kalman filter assuming that
process noise and measurement noise are correlated as defined in
the `self.M` matrix.
If z is None, nothing is changed.
Parameters
----------
z : (dim_z, 1): array_like
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be convertible to a column vector.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
H : np.array, or None
Optionally provide H to override the measurement function for this
one call, otherwise self.H will be used.
"""
if z is None:
return
z = reshape_z(z, self.dim_z, self.x.ndim)
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
# rename for readability and a tiny extra bit of speed
if H is None:
H = self.H
# handle special case: if z is in form [[z]] but x is not a column
# vector dimensions will not match
if self.x.ndim == 1 and shape(z) == (1, 1):
z = z[0]
if shape(z) == (): # is it scalar, e.g. z=3 or z=np.array(3)
z = np.asarray([z])
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, self.x)
# common subexpression for speed
PHT = dot(self.P, H.T)
# project system uncertainty into measurement space
self.S = dot(H, PHT) + dot(H, self.M) + dot(self.M.T, H.T) + R
# K = PH'inv(S)
# map system uncertainty into kalman gain
self.K = dot(PHT + self.M, self.inv(self.S))
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = self.x + dot(self.K, self.y)
self.P = self.P - dot(self.K, dot(H, self.P) + self.M.T)
self.z = np.copy(z) # save the measurement
if self.compute_log_likelihood:
self.log_likelihood = logpdf(x=self.y, cov=self.S)
self.likelihood = math.exp(self.log_likelihood)
if self.likelihood == 0:
self.likelihood = sys.float_info.min
def update_steadystate(self, z):
"""
Add a new measurement (z) to the Kalman filter without recomputing
the Kalman gain K, the state covariance P, or the system
uncertainty S.
You can use this for LTI systems since the Kalman gain and covariance
converge to a fixed value. Precompute these and assign them explicitly,
or run the Kalman filter using the normal predict()/update(0 cycle
until they converge.
The main advantage of this call is speed. We do significantly less
computation, notably avoiding a costly matrix inversion.
Use in conjunction with predict_steadystate(), otherwise P will grow
without bound.
Parameters
----------
z : (dim_z, 1): array_like
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be convertible to a column vector.
Examples
--------
>>> cv = kinematic_kf(dim=3, order=2) # 3D const velocity filter
>>> # let filter converge on representative data, then save k and P
>>> for i in range(100):
>>> cv.predict()
>>> cv.update([i, i, i])
>>> saved_k = np.copy(cv.K)
>>> saved_P = np.copy(cv.P)
later on:
>>> cv = kinematic_kf(dim=3, order=2) # 3D const velocity filter
>>> cv.K = np.copy(saved_K)
>>> cv.P = np.copy(saved_P)
>>> for i in range(100):
>>> cv.predict_steadystate()
>>> cv.update_steadystate([i, i, i])
"""
if z is None:
return
z = reshape_z(z, self.dim_z, self.x.ndim)
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(self.H, self.x)
if self.compute_log_likelihood:
S = dot(dot(self.H, self.P), self.H.T) + self.R
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = self.x + dot(self.K, self.y)
self.z = np.copy(z) # save the measurement
if self.compute_log_likelihood:
self.log_likelihood = logpdf(x=self.y, cov=S)
self.likelihood = math.exp(self.log_likelihood)
if self.likelihood == 0:
self.likelihood = sys.float_info.min
def update(self, z, R=None, H=None):
"""
Add a new measurement (z) to the Kalman filter. If z is None, nothing
is changed.
Parameters
----------
z : (dim_z, 1): array_like
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be convertible to a column vector.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
H : np.array, or None
Optionally provide H to override the measurement function for this
one call, otherwise self.H will be used.
"""
if z is None:
return
z = reshape_z(z, self.dim_z, self.x.ndim)
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
if H is None:
H = self.H
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, self.x)
# common subexpression for speed
PHT = dot(self.P, H.T)
# S = HPH' + R
# project system uncertainty into measurement space
self.S = dot(H, PHT) + R
# K = PH'inv(S)
# map system uncertainty into kalman gain
self.K = dot(PHT, self.inv(self.S))
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = self.x + dot(self.K, self.y)
# P = (I-KH)P(I-KH)' + KRK'
# This is more numerically stable
# and works for non-optimal K vs the equation
# P = (I-KH)P usually seen in the literature.
I_KH = self._I - dot(self.K, H)
self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T)
self.z = np.copy(z) # save the measurement
if self.compute_log_likelihood:
self.log_likelihood = logpdf(x=self.y, cov=self.S)
self.likelihood = math.exp(self.log_likelihood)
if self.likelihood == 0:
self.likelihood = sys.float_info.min
def update(x, P, z, R, H=None, return_all=False):
"""
Add a new measurement (z) to the Kalman filter. If z is None, nothing
is changed.
This can handle either the multidimensional or unidimensional case. If
all parameters are floats instead of arrays the filter will still work,
and return floats for x, P as the result.
update(1, 2, 1, 1, 1) # univariate
update(x, P, 1
Parameters
----------
x : numpy.array(dim_x, 1), or float
State estimate vector
P : numpy.array(dim_x, dim_x), or float
Covariance matrix
z : (dim_z, 1): array_like
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be convertible to a column vector.
R : numpy.array(dim_z, dim_z), or float
Measurement noise matrix
H : numpy.array(dim_x, dim_x), or float, optional
Measurement function. If not provided, a value of 1 is assumed.
return_all : bool, default False
If true, y, K, S, and log_likelihood are returned, otherwise
only x and P are returned.
Returns
-------
x : numpy.array
Posterior state estimate vector
P : numpy.array
Posterior covariance matrix
y : numpy.array or scalar
Residua. Difference between measurement and state in measurement space
K : numpy.array
Kalman gain
S : numpy.array
System uncertainty in measurement space
log_likelihood : float
log likelihood of the measurement
"""
#pylint: disable=bare-except
if z is None:
if return_all:
return x, P, None, None, None, None
return x, P
if H is None:
H = np.array([1])
if np.isscalar(H):
H = np.array([H])
Hx = np.atleast_1d(dot(H, x))
z = reshape_z(z, Hx.shape[0], x.ndim)
# error (residual) between measurement and prediction
y = z - Hx
# project system uncertainty into measurement space
S = dot(dot(H, P), H.T) + R
# map system uncertainty into kalman gain
try:
K = dot(dot(P, H.T), linalg.inv(S))
except:
# can't invert a 1D array, annoyingly
K = dot(dot(P, H.T), 1. / S)
# predict new x with residual scaled by the kalman gain
x = x + dot(K, y)
# P = (I-KH)P(I-KH)' + KRK'
KH = dot(K, H)
try:
I_KH = np.eye(KH.shape[0]) - KH
except:
I_KH = np.array([1 - KH])
P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
if return_all:
# compute log likelihood
log_likelihood = logpdf(z, dot(H, x), S)
return x, P, y, K, S, log_likelihood
return x, P
def update(self, z, R=None, H=None, timestamp=-1):
# print(self.id, " update !")
if timestamp == -1:
warnings.warn(
"timestamp not specified: default time increment will be used")
# timestep set to last time difference between subsequent measurements
self.dt = DEFAULT_TIME_INCREMENT
self.prev_ti = self.curr_ti
self.curr_ti += DEFAULT_TIME_INCREMENT
else:
# timestep set to last time difference between subsequent measurements
self.prev_ti = self.curr_ti
self.curr_ti = timestamp
self.dt = timestamp - self.curr_ti
# data validation
v = z - dot(self.H, self.x_prior)
S = self.R + dot(self.H, dot(self.P_prior, self.H.T))
validation_region = dot(v.T, dot(self.inv(S), v))
# print("nodal validation region = ", validation_region)
if validation_region >= self.NODAL_VALIDATION_BOUND:
print("rejected nodal validation region:", validation_region)
return True
# set to None to force recompute
self._log_likelihood = None
self._likelihood = None
self._mahalanobis = None
if z is None:
self.z = np.array([[None] * self.dim_z]).T
self.x_post = self.x.copy()
self.P_post = self.P.copy()
self.y = zeros((self.dim_z, 1))
return
z = reshape_z(z, self.dim_z, self.x.ndim)
self.z_current = z
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
if H is None:
H = self.H
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, self.x)
# inverse of R, used in information form of variance update and Kalman gain
RI = self.inv(R)
# information form of variance update equation
# PI = PI + HT*RI*H
self.PI = self.PI + dot(dot(H.T, RI), H)
self.PI_post = self.PI.copy()
# needed for Kalman gain matrix
self.P = self.inv(self.PI)
# Kalman gain matrix
# W = P*HT*RI
self.W = dot(dot(self.P, H.T), RI)
# x = x + Wy
self.x = self.x + dot(self.W, self.y)
# update x_post & P_post
self.x_post = self.x
self.P_post = self.P
