CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place.

| Download
Project: test
Views: 91872
Tweet
Share
Share
1
# -*- coding: utf-8 -*-

2
"""

3
Created on Fri Feb 13 17:47:56 2015

4


5
@author: rlabbe

6
"""

7
import numpy as np

8
from filterpy.kalman import  UnscentedKalmanFilter as UKF

9
from math import atan2, radians,degrees

10
from filterpy.common import stats

11
import matplotlib.pyplot as plt

12


13
p = (-10, -10)  

14


15
def hx(x):

16
    dx = x[0] - hx.p[0]

17
    dy = x[1] - hx.p[1]    

18
    return np.array([atan2(dy,dx), (dx**2 + dy**2)**.5])

19
    

20
def fx(x,dt):

21
    return x

22
    

23


24


25
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, kappa=2.)

26


27
kf.x = np.array([100, 100.])

28
kf.P *= 40

29
hx.p = kf.x - np.array([50,50])

30


31
d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5

32


33
stats.plot_covariance_ellipse(

34
       kf.x, cov=kf.P, axis_equal=True, 

35
       facecolor='y', edgecolor=None, alpha=0.6)

36
plt.scatter([100], [100], c='y', label='Initial')

37


38
kf.R[0,0] = radians (1)**2

39
kf.R[1,1] = 2.**2

40


41


42
kf.predict()

43
kf.update(np.array([radians(45), d]))

44


45
print(kf.x)

46
print(kf.P)

47


48
stats.plot_covariance_ellipse(

49
       kf.x, cov=kf.P, axis_equal=True, 

50
       facecolor='g', edgecolor=None, alpha=0.6)

51
plt.scatter([100], [100], c='g', label='45 degrees')

52
       

53
       

54
p = (13, -11)

55
hx.p = kf.x - np.array([-50,50])

56
d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5

57


58
kf.predict()

59
kf.update(np.array([radians(135), d]))

60


61
stats.plot_covariance_ellipse(

62
       kf.x, cov=kf.P, axis_equal=True, 

63
       facecolor='b', edgecolor=None, alpha=0.6)

64
plt.scatter([100], [100], c='b', label='135 degrees')

65


66
plt.legend(scatterpoints=1, markerscale=3)

67