def main():
deltaTime = 0.001
numSteps = 500
F = np.array([[1, 0], [0, 1]]) * 0
B = np.array([[1, 0], [0, 1]])
H = np.array([[1, 0], [0, 1]])
P = np.array([[0, 0], [0, 0]])
p_sigma = 1e-3
o_sigma = 1e-3
Q = np.array([[1, 0], [0, 1]]) * (p_sigma**2)
R = np.array([[1, 0], [0, 1]]) * (o_sigma**2)
t = (np.arange(0, numSteps) * 0.01)
i = np.zeros((2, numSteps))
i[0, 0:] = np.sin(2 * np.pi * 1 * t) * 1
p_n = np.reshape(
np.random.multivariate_normal([0, 0], [[p_sigma, 0], [0, p_sigma]],
numSteps), (2, numSteps))
o_n = np.reshape(
np.random.multivariate_normal([0, 0], [[o_sigma, 0], [0, o_sigma]],
numSteps), (2, numSteps))
x = np.zeros((2, numSteps)) #process output
y = np.zeros((2, numSteps)) #process output
x_pred = np.zeros((2, numSteps))
KF = KalmanFilter(F, B, Q, H, R, P, x[0:, 0])
for t in range(1, numSteps):
x[0:,
t] = B.dot(i[0:, t]) + p_n[0:, t] #process output with process noise
KF.predict(i[0:, t])
x_pred[0:, t], P = KF.getPrediction() #predicting next process state
#print(P)
y[0:, t] = h(x[0:, t]) + o_n[0:, t] #sensor output
KF.update(y[0:, t]) #update Kalman
print(np.sum((y - x_pred)**2) / numSteps)
print(np.sum((x - x_pred)**2) / numSteps)
plt.plot(x[0, 0:], 'r', y[0, 0:], 'b', x_pred[0, 0:], 'g')
plt.show()
def main(): deltaTime=0.01 numSteps=5000 x_0=np.array([[0],[0]]) F=np.array([[1, deltaTime],[0, 1]]) B=np.array([[0, 0.5*deltaTime**2],[0, deltaTime]]) H=np.array([[1, 0],[0, 1]]) P=np.array([[0, 0],[0, 0]]) p_sigma=1e-7 o_sigma=1e-7 Q=np.array([[0.25*deltaTime**4, 0.5*deltaTime**3],[0.5*deltaTime**3, deltaTime**2]])*(p_sigma**2) R=np.array([[1, 0],[0, 1]])*(o_sigma**2) t=(np.arange(0,numSteps)*deltaTime) i=np.zeros((2,numSteps)) i[1,0:]=np.cos(2*np.pi*0.1*t)*10 p_n=np.reshape(np.random.multivariate_normal([0, 0],[[p_sigma,0],[0,p_sigma]],numSteps),(2,numSteps))*0 o_n=np.reshape(np.random.multivariate_normal([0, 0],[[o_sigma,0],[0,o_sigma]],numSteps),(2,numSteps))*0 x=np.zeros((2,numSteps)) y=np.zeros((2,numSteps)) x_pred=np.zeros((2,numSteps)) x[0,0]=0 x[1,0]=0 KF=KalmanFilter(F,B,Q,H,R,P,x[0:,0]) for t in range(1,numSteps): x[0:,t]=F.dot(x[0:,t-1])+B.dot(i[0:,t])+p_n[0:,t] KF.predict(i[0:,t]) x_pred[0:,t],P=KF.getPrediction() #print(P) y[0:,t]=H.dot(x[0:,t])+o_n[0:,t] KF.update(y[0:,t]) print(np.sum((y-x_pred)**2)/numSteps) print(np.sum((x-x_pred)**2)/numSteps) plt.plot(i[1,0:],'k',x[0,0:],'r',y[0,0:],'b', x_pred[0,0:],'g') plt.show()