История изменений

Исправление shkolnick-kun, 14.12.18 21:04 (текущая версия) :

Это базовый класс (не полный).

Тебе надо класс robust_lin.


import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random

from sqrtkf import robust_lin as kalman

#Начальное приближение состояния
x = np.array([1.0,0,0,0]).astype('float32')

#Предполагаемая матрица ковариации состояния
P = 0.001*np.eye(4).astype('float32')

#Матрица переходов (в данном случае модель - поступательное движение)
dt = 20.0
F = np.array([[1, dt, 0,  0],
            [0,  1, 0,  0],
            [0,  0, 1, dt],
            [0,  0, 0,  1]]).astype('float32')

#bf = np.array([20.0,0,0,0]).astype('float32')

#Матрица возмущений
B = np.eye(4).astype('float32')

#Матрица ковариации возмущений
Q = 0.0000001*np.eye(4).astype('float32')

#B = np.array([[1., 0],[1, 0],[0, 1],[0, 1]])
#Q = 0.0000001*np.eye(2).astype('float32')

#Матрица наблюдения
H = np.array([[1.0, 0, 0, 0],
              [0, 0, 1, 0]]).astype('float32')

#Вектор смещения наблюдений
bh = np.array([20.0, 0.0]).astype('float32')

def _noise():
    A = 20.1
    B = 20.3
    a = np.array([random.randn()*A, random.randn()*A])
    b = random.randn()*B
    c = np.array([b, -b])
    return a + c

ri = 100.
rj = -0.5*ri

#Матрица ковариации шума наблюдений
R = np.array([[ri,rj],
            [rj, ri]])

kf = kalman(x, P, Q, R)

xs = []
ys = []

xz = []
yz = []

xo = []
yo = []


n = []
e = []
count = 1500
'''
for i in range(count):
    b = 0.0
    c = random.randn()*0.5
    a = 1*i/(1+0.00001*i*i)
    A = 100
    #z = np.array([a*np.cos(0.01*i)+random.randn()*b+c, a*np.sin(0.01*i)+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([a*np.cos(0.01*i*(1+0.0001*i))+random.randn()*b+c, a*np.sin(0.01*i)+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([A*np.cos(0.01*i)+random.randn()*b + c, A*np.sin(0.0101*i*(1+0.0001*i))+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([A*np.cos(0.01*i)+random.randn()*b + c, A*np.sin(0.011*i)+random.randn()*b+c]).transpose().astype(float)
    
    
    p = kf.run(F, bf, B, H, bh, z)
    
    xs.append (p[0])
    ys.append (p[1])
    
    xz.append (z[0])
    yz.append (z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i)
'''

z = np.array([0, 0]).transpose().astype(float)

for i in range(count//4):
    z += np.array([1.0, 2.0])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i)

for i in range(count//4):
    z += np.array([-1.0, 3.0])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+count//4)
    
for i in range(count//4):
    
    c = random.randn()*1.7
    z += np.array([-5.0+c, -3.0+c])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+2*count//4)
    
for i in range(count//4):
    
    c = random.randn()*0.5
    z += np.array([1+c, -2+c]).transpose().astype(float)
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+3*count//4)


plt.plot(xs, ys)
plt.plot(xz, yz)
plt.show()

plt.plot(xo, yo)
plt.show()

plt.plot(n, e)
plt.show()

print(np.median(e))

Во, попробуй с «НЛО» поиграться поймешь, что как. Только начать лучше с bierman_lin.

Исправление shkolnick-kun, 14.12.18 20:59:

Это базовый класс (не полный).

Тебе надо класс robust_lin.


import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random

from sqrtkf import robust_lin as kalman

#Начальное приближение состояния
x = np.array([1.0,0,0,0]).astype('float32')

#Предполагаемая матрица ковариации состояния
P = 0.001*np.eye(4).astype('float32')

#Матрица переходов (в данном случае модель - поступательное движение)
dt = 20.0
F = np.array([[1, dt, 0,  0],
            [0,  1, 0,  0],
            [0,  0, 1, dt],
            [0,  0, 0,  1]]).astype('float32')

#bf = np.array([20.0,0,0,0]).astype('float32')

#Матрица возмущений
B = np.eye(4).astype('float32')

#Матрица ковариации возмущений
Q = 0.0000001*np.eye(4).astype('float32')

#B = np.array([[1., 0],[1, 0],[0, 1],[0, 1]])
#Q = 0.0000001*np.eye(2).astype('float32')

#Матрица наблюдения
H = np.array([[1.0, 0, 0, 0],
              [0, 0, 1, 0]]).astype('float32')

#Вектор смещения наблюдений
bh = np.array([20.0, 0.0]).astype('float32')

def _noise():
    A = 20.1
    B = 20.3
    a = np.array([random.randn()*A, random.randn()*A])
    b = random.randn()*B
    c = np.array([b, -b])
    return a + c

ri = 100.
rj = -0.5*ri

#Матрица ковариации шума наблюдений
R = np.array([[ri,rj],
            [rj, ri]])

kf = kalman(x, P, Q, R)

xs = []
ys = []

xz = []
yz = []

xo = []
yo = []


n = []
e = []
count = 1500
'''
for i in range(count):
    b = 0.0
    c = random.randn()*0.5
    a = 1*i/(1+0.00001*i*i)
    A = 100
    #z = np.array([a*np.cos(0.01*i)+random.randn()*b+c, a*np.sin(0.01*i)+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([a*np.cos(0.01*i*(1+0.0001*i))+random.randn()*b+c, a*np.sin(0.01*i)+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([A*np.cos(0.01*i)+random.randn()*b + c, A*np.sin(0.0101*i*(1+0.0001*i))+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([A*np.cos(0.01*i)+random.randn()*b + c, A*np.sin(0.011*i)+random.randn()*b+c]).transpose().astype(float)
    
    
    p = kf.run(F, bf, B, H, bh, z)
    
    xs.append (p[0])
    ys.append (p[1])
    
    xz.append (z[0])
    yz.append (z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i)
'''

z = np.array([0, 0]).transpose().astype(float)

for i in range(count//4):
    z += np.array([1.0, 2.0])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i)

for i in range(count//4):
    z += np.array([-1.0, 3.0])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+count//4)
    
for i in range(count//4):
    
    c = random.randn()*1.7
    z += np.array([-5.0+c, -3.0+c])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+2*count//4)
    
for i in range(count//4):
    
    c = random.randn()*0.5
    z += np.array([1+c, -2+c]).transpose().astype(float)
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+3*count//4)


plt.plot(xs, ys)
plt.plot(xz, yz)
plt.show()

plt.plot(xo, yo)
plt.show()

plt.plot(n, e)
plt.show()

print(np.median(e))

Во, попробуй с «НЛО» поиграться поймешь, что как.

Исходная версия shkolnick-kun, 14.12.18 20:57:

Это базовый класс (не полный).

Тебе надо класс robust_lin.


import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random

from sqrtkf import robust_lin as kalman

#Начальное приближение состояния
x = np.array([1.0,0,0,0]).astype('float32')

#Предполагаемая матрица ковариации состояния
P = 0.001*np.eye(4).astype('float32')

#Матрица переходов (в данном случае модель - поступательное движение)
dt = 20.0
F = np.array([[1, dt, 0,  0],
            [0,  1, 0,  0],
            [0,  0, 1, dt],
            [0,  0, 0,  1]]).astype('float32')

#bf = np.array([20.0,0,0,0]).astype('float32')

#Матрица возмущений
B = np.eye(4).astype('float32')

#Матрица ковариации возмущений
Q = 0.0000001*np.eye(4).astype('float32')

#B = np.array([[1., 0],[1, 0],[0, 1],[0, 1]])
#Q = 0.0000001*np.eye(2).astype('float32')

#Матрица наблюдения
H = np.array([[1.0, 0, 0, 0],
              [0, 0, 1, 0]]).astype('float32')

#Вектор смещения наблюдений
bh = np.array([20.0, 0.0]).astype('float32')

def _noise():
    A = 20.1
    B = 20.3
    a = np.array([random.randn()*A, random.randn()*A])
    b = random.randn()*B
    c = np.array([b, -b])
    return a + c

ri = 100.
rj = -0.5*ri

#Матрица ковариации шума наблюдений
R = np.array([[ri,rj],
            [rj, ri]])

kf = kalman(x, P, Q, R)

xs = []
ys = []

xz = []
yz = []

xo = []
yo = []


n = []
e = []
count = 1500
'''
for i in range(count):
    b = 0.0
    c = random.randn()*0.5
    a = 1*i/(1+0.00001*i*i)
    A = 100
    #z = np.array([a*np.cos(0.01*i)+random.randn()*b+c, a*np.sin(0.01*i)+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([a*np.cos(0.01*i*(1+0.0001*i))+random.randn()*b+c, a*np.sin(0.01*i)+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([A*np.cos(0.01*i)+random.randn()*b + c, A*np.sin(0.0101*i*(1+0.0001*i))+random.randn()*b+c]).transpose().astype(float)
    #z = np.array([A*np.cos(0.01*i)+random.randn()*b + c, A*np.sin(0.011*i)+random.randn()*b+c]).transpose().astype(float)
    
    
    p = kf.run(F, bf, B, H, bh, z)
    
    xs.append (p[0])
    ys.append (p[1])
    
    xz.append (z[0])
    yz.append (z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i)
'''

z = np.array([0, 0]).transpose().astype(float)

for i in range(count//4):
    z += np.array([1.0, 2.0])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i)

for i in range(count//4):
    z += np.array([-1.0, 3.0])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+count//4)
    
for i in range(count//4):
    
    c = random.randn()*1.7
    z += np.array([-5.0+c, -3.0+c])
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+2*count//4)
    
for i in range(count//4):
    
    c = random.randn()*0.5
    z += np.array([1+c, -2+c]).transpose().astype(float)
    o = z + _noise()
    
    p = kf.run(F, B, H, o, bh = bh)

    xs.append(p[0])
    ys.append(p[1])
    
    xo.append(o[0])
    yo.append(o[1])
    
    xz.append(z[0])
    yz.append(z[1])
    
    e.append(np.linalg.norm(z - p))
    #e.append(z - p)
    n.append(i+3*count//4)


plt.plot(xs, ys)
plt.plot(xz, yz)
plt.show()

plt.plot(xo, yo)
plt.show()

plt.plot(n, e)
plt.show()

print(np.median(e))