Step 2: Create a KalmanFilter object with these parameters.
Step 3: Simulate the movement of the object for a number of time steps, generating true states and measurements.
Step 3: For each measurement, predict the next state using kf.predict().
Step 4: Update the state estimate based on the measurement using kf.update().
Step 5: Store the estimated state in a list.
Step 6: Plot the true and estimated positions.
class KalmanFilter:
def __init__(self, F, H, Q, R, x0, P0):
self.F = F
self.H = H
self.Q =Q
self.R=R
self.x = x0
self.P=P0
def predict(self):
self.x = np.dot(self.F, self.x)
self.P = np.dot (np.dot (self.F, self.P), self.F.T) + self.Q
def update (self, z):
y=z-np.dot (self.H, self.x)
S = np.dot (np. dot (self.H, self.P), self.H.T)+ self.R
K = np.dot (np.dot (self.P, self.H.T),np.linalg.inv(S))
self.x = self.x + np.dot (K, y)
self.P = np.dot (np.eye(self.F.shape[0])- np.dot (K, self.H), self.P)
dt = 0.1
F = np.array([[1, dt], [0, 1]])
H = np.array([[1, 0]])
Q = np.diag([0.1, 0.1])
R = np.array([[1]])
x0 = np.array([0, 0])
P0 = np.diag ([1, 1])
kf=KalmanFilter(F,H,Q,R,x0,P0)
measurements = []
for i in range(100):
true_states.append([i*dt, 1])
measurements.append(i*dt +np.random.normal(scale=1))
est_states = []
for z in measurements:
kf.predict()
kf.update(np.array([z]))
est_states.append(kf.x)
import matplotlib.pyplot as plt
plt.plot([s[0] for s in true_states], label='true')
plt.plot([s[0] for s in est_states],label='estimate')
plt.legend ()
plt.show()
![aai 5-1](https://private-user-images.githubusercontent.com/94619247/312305148-90c1b368-29f2-480a-a407-4f1c71e07992.png?jwt=eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJpc3MiOiJnaXRodWIuY29tIiwiYXVkIjoicmF3LmdpdGh1YnVzZXJjb250ZW50LmNvbSIsImtleSI6ImtleTUiLCJleHAiOjE3MjIyMzQzNTIsIm5iZiI6MTcyMjIzNDA1MiwicGF0aCI6Ii85NDYxOTI0Ny8zMTIzMDUxNDgtOTBjMWIzNjgtMjlmMi00ODBhLWE0MDctNGYxYzcxZTA3OTkyLnBuZz9YLUFtei1BbGdvcml0aG09QVdTNC1ITUFDLVNIQTI1NiZYLUFtei1DcmVkZW50aWFsPUFLSUFWQ09EWUxTQTUzUFFLNFpBJTJGMjAyNDA3MjklMkZ1cy1lYXN0LTElMkZzMyUyRmF3czRfcmVxdWVzdCZYLUFtei1EYXRlPTIwMjQwNzI5VDA2MjA1MlomWC1BbXotRXhwaXJlcz0zMDAmWC1BbXotU2lnbmF0dXJlPTE2NjUwMTk4NWM1MmQ3M2Q0N2FjM2U0OTM2NDljNWY2M2JkNWQ3YWE5MjhjYTU0MmQ1ZmQ2MWM1MzkyYWQwYjgmWC1BbXotU2lnbmVkSGVhZGVycz1ob3N0JmFjdG9yX2lkPTAma2V5X2lkPTAmcmVwb19pZD0wIn0.fb_n1LlN43hIpOv7eFrMR-XYSx8il6vRZ4DYhJRWq6Y)