i have following class in python:
import numpy class kalmanfilterlinear: def __init__(self,_a, _b, _h, _x, _p, _q, _r): self.a = _a # state transition matrix. self.b = _b # control matrix. self.h = _h # observation matrix. self.current_state_estimate = _x # initial state estimate. self.current_prob_estimate = _p # initial covariance estimate. self.q = _q # estimated error in process. self.r = _r # estimated error in measurements. def getcurrentstate(self): return self.current_state_estimate def step(self,control_vector,measurement_vector): #---------------------------prediction step----------------------------- print self.a print self.current_state_estimate print self.b print control_vector predicted_state_estimate = self.a * self.current_state_estimate + self.b * control_vector predicted_prob_estimate = (self.a * self.current_prob_estimate) * numpy.transpose(self.a) + self.q #--------------------------observation step----------------------------- innovation = measurement_vector - self.h*predicted_state_estimate innovation_covariance = self.h*predicted_prob_estimate*numpy.transpose(self.h) + self.r #-----------------------------update step------------------------------- kalman_gain = predicted_prob_estimate * numpy.transpose(self.h) * numpy.linalg.inv(innovation_covariance) self.current_state_estimate = predicted_state_estimate + kalman_gain * innovation # need size of matrix can make identity matrix. size = self.current_prob_estimate.shape[0] # eye(n) = nxn identity matrix. self.current_prob_estimate = (numpy.eye(size)-kalman_gain*self.h)*predicted_prob_estimate i initialize this:
import numpy np state_transition = np.matrix([[1,-1.0/400],[0,1]]) control_matrix = np.matrix([ [1.0/400, 0.0], [0.0, 0.0] ]) observation_matrix = np.eye(2) observation_matrix[1][1] = 0.0 initial_probability = np.eye(2) process_covariance = np.zeros(2) measurement_covariance = np.eye(2)*0.2 initial_state = np.matrix([[ins_h],[0.0]]) kf = kalmanfilterlinear(state_transition, control_matrix, observation_matrix, initial_state, initial_probability, process_covariance, measurement_covariance) then run code check works:
kf.step(initial_state, initial_state ) and error:
predicted_state_estimate = self.a * self.current_state_es... error: can't multiply sequence non-int of type 'float' i don't have experience numpy , i'd grateful help.
Comments
Post a Comment