Robotics, Kalman Filter, Robot Operating System, State Estimation, Artificial Intelligence.
= self.Sigma.dot(self.C.T).dot(np.linalg.inv(self.C.dot(self.Sigma).dot(self.C.T) + self.Q)) # Update state estimate (mu) self.mu = self.mu + K.dot(z - self.C.dot(self.mu)) # Update covariance (Sigma) self.Sigma = (np.eye(len(K)) - K.dot(self.C)).dot(self.Sigma) return self.mu, self.Sigma Running