Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
Download
96131 views
Tweet
Share
Share
1
# -*- coding: utf-8 -*-

2
"""Copyright 2015 Roger R Labbe Jr.

3


4
FilterPy library.

5
http://github.com/rlabbe/filterpy

6


7
Documentation at:

8
https://filterpy.readthedocs.org

9


10
Supporting book at:

11
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

12


13
This is licensed under an MIT license. See the readme.MD file

14
for more information.

15
"""

16


17
from __future__ import (absolute_import, division, print_function,

18
                        unicode_literals)

19


20


21
from filterpy.common import linear_ode_discretation, Q_discrete_white_noise

22
from numpy import array

23


24
def near_eq(x,y):

25
    return abs(x-y) < 1.e-18

26


27


28
def test_Q_discrete_white_noise():

29


30
    Q = Q_discrete_white_noise (2)

31
    assert Q[0,0] == .25

32
    assert Q[1,0] == .5

33
    assert Q[0,1] == .5

34
    assert Q[1,1] == 1

35


36
    assert Q.shape == (2,2)

37


38


39
def test_linear_ode():

40


41
    F = array([[0,0,1,0,0,0],

42
               [0,0,0,1,0,0],

43
               [0,0,0,0,1,0],

44
               [0,0,0,0,0,1],

45
               [0,0,0,0,0,0],

46
               [0,0,0,0,0,0]], dtype=float)

47


48
    L = array ([[0,0],

49
                [0,0],

50
                [0,0],

51
                [0,0],

52
                [1,0],

53
                [0,1]], dtype=float)

54


55
    q = .2

56
    Q = array([[q, 0],[0, q]])

57
    dt = 0.5

58
    A,Q = linear_ode_discretation(F, L, Q, dt)

59


60
    val = [1, 0, dt, 0, 0.5*dt**2, 0]

61


62
    for i in range(6):

63
        assert val[i] == A[0,i]

64


65
    for i in range(6):

66
        assert val[i-1] == A[1,i] if i > 0 else A[1,i] == 0

67


68
    for i in range(6):

69
        assert val[i-2] == A[2,i] if i > 1 else A[2,i] == 0

70


71
    for i in range(6):

72
        assert val[i-3] == A[3,i] if i > 2 else A[3,i] == 0

73


74


75
    for i in range(6):

76
        assert val[i-4] == A[4,i] if i > 3 else A[4,i] == 0

77


78
    for i in range(6):

79
        assert val[i-5] == A[5,i] if i > 4 else A[5,i] == 0

80


81
    assert near_eq(Q[0,0], (1./20)*(dt**5)*q)

82
    assert near_eq(Q[0,1], 0)

83
    assert near_eq(Q[0,2], (1/8)*(dt**4)*q)

84
    assert near_eq(Q[0,3], 0)

85
    assert near_eq(Q[0,4], (1./6)*(dt**3)*q)

86
    assert near_eq(Q[0,5], 0)

87


88


89


90
if __name__ == "__main__":

91
    test_linear_ode()

92
    test_Q_discrete_white_noise()

93


94
    F = array([[0,0,1,0,0,0],

95
               [0,0,0,1,0,0],

96
               [0,0,0,0,1,0],

97
               [0,0,0,0,0,1],

98
               [0,0,0,0,0,0],

99
               [0,0,0,0,0,0]], dtype=float)

100


101
    L = array ([[0,0],

102
                [0,0],

103
                [0,0],

104
                [0,0],

105
                [1,0],

106
                [0,1]], dtype=float)

107


108
    q = .2

109
    Q = array([[q, 0],[0, q]])

110
    dt = 1/30

111
    A,Q = linear_ode_discretation(F, L, Q, dt)

112


113
    print(Q)

114