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
import numpy.random as random

21
from numpy.random import randn

22
import numpy as np

23
import matplotlib.pyplot as plt

24
from filterpy.kalman import KalmanFilter

25
from numpy import array, asarray

26


27
DO_PLOT = False

28
SEED = 1124

29


30


31
def single_measurement_test():

32
    dt = 0.1

33
    sigma = 2.

34


35
    kf2 = KalmanFilter(dim_x=2, dim_z=1)

36


37
    kf2.F = array ([[1., dt], [0., 1.]])

38
    kf2.H = array ([[1., 0.]])

39
    kf2.x = array ([[0.], [1.]])

40
    kf2.Q = array ([[dt**3/3, dt**2/2],

41
                    [dt**2/2, dt]]) * 0.02

42
    kf2.P *= 100

43
    kf2.R[0,0] = sigma**2

44


45
    random.seed(SEED)

46
    xs = []

47
    zs = []

48
    nom = []

49
    for i in range(1, 100):

50
        m0 = i + randn()*sigma

51
        z = array([[m0]])

52


53
        kf2.predict()

54
        kf2.update(z)

55


56
        xs.append(kf2.x.T[0])

57
        zs.append(z.T[0])

58
        nom.append(i)

59


60
    xs = asarray(xs)

61
    zs = asarray(zs)

62
    nom = asarray(nom)

63


64


65
    res = nom-xs[:,0]

66
    std_dev = np.std(res)

67
    print('std: {:.3f}'.format (std_dev))

68


69
    global DO_PLOT

70
    if DO_PLOT:

71


72
        plt.subplot(211)

73
        plt.plot(xs[:,0])

74
        #plt.plot(zs[:,0])

75


76


77
        plt.subplot(212)

78
        plt.plot(res)

79
        plt.show()

80


81
    return std_dev

82


83


84
def sensor_fusion_test(wheel_sigma=2., gps_sigma=4.):

85
    dt = 0.1

86


87
    kf2 = KalmanFilter(dim_x=2, dim_z=2)

88


89
    kf2.F = array ([[1., dt], [0., 1.]])

90
    kf2.H = array ([[1., 0.], [1., 0.]])

91
    kf2.x = array ([[0.], [0.]])

92
    kf2.Q = array ([[dt**3/3, dt**2/2],

93
                    [dt**2/2, dt]]) * 0.02

94
    kf2.P *= 100

95
    kf2.R[0,0] = wheel_sigma**2

96
    kf2.R[1,1] = gps_sigma**2

97


98


99
    random.seed(SEED)

100
    xs = []

101
    zs = []

102
    nom = []

103
    for i in range(1, 100):

104
        m0 = i + randn()*wheel_sigma

105
        m1 = i + randn()*gps_sigma

106
        if gps_sigma >1e40:

107
            m1 = -1e40

108


109
        z = array([[m0], [m1]])

110


111
        kf2.predict()

112
        kf2.update(z)

113


114
        xs.append(kf2.x.T[0])

115
        zs.append(z.T[0])

116
        nom.append(i)

117


118
    xs = asarray(xs)

119
    zs = asarray(zs)

120
    nom = asarray(nom)

121


122


123
    res = nom-xs[:,0]

124
    std_dev = np.std(res)

125
    print('fusion std: {:.3f}'.format (np.std(res)))

126


127
    if DO_PLOT:

128


129
        plt.subplot(211)

130
        plt.plot(xs[:,0])

131
        #plt.plot(zs[:,0])

132
        #plt.plot(zs[:,1])

133


134
        plt.subplot(212)

135
        plt.axhline(0)

136
        plt.plot(res)

137
        plt.show()

138


139
    print(kf2.Q)

140
    print(kf2.K)

141
    return std_dev

142


143


144
def test_fusion():

145
    std1 = sensor_fusion_test()

146
    std2 = single_measurement_test()

147
    assert (std1 < std2)

148


149
if __name__ == "__main__":

150
    DO_PLOT=True

151
    sensor_fusion_test(2,4e100)

152
    single_measurement_test()

153


154