https://brain2net.ru/

Вернуться на страницу brain2net:

https://brain2net.ru/post/model-prostogo-osczillyatora/#more-67

Модель простого осциллятора

In [1]:
import numpy as np
import pandas as pd
import random
import matplotlib.pyplot as plt

Простейшая функция, описывающая один шаг решения уравнения осциллятора методом Эйлера

In [2]:
def step(x, dt, gamma = 0., omega0 = 1., f = 0.):  
    x[1] =  - omega0**2 * x[0] * dt+ (1 - 2 * gamma * dt) * x[1] + f
    x[0] = x[0] + x[1] * dt
    return x

Решение уравнения простого осциллятора со слабым затуханием

In [3]:
x = np.array([[1.],[0.]])
dt = 0.005

position = [x[0,0]]
velosity = [x[1,0]]
time = [0]

gamma = 0.05
omega0 = 1.
for i in range(10000):

    result = step(x, dt, gamma = gamma, omega0 = omega0)

    position += [result[0,0]]
    velosity += [result[1,0]]
    time += [dt * i]
In [4]:
plt.figure(figsize=(18,8))
plt.plot(time, position)
plt.grid()

На осциллятор каждые dtau действует случайное возмудение с мат. ожиданием 0 и f_std

In [5]:
x = np.array([[1.],[0.]])
dt = 0.005
dtau = 0.004
f_std = 10.

position = [x[0,0]]
velosity = [x[1,0]]
time = [0]
tau = 0.

gamma = 0.05
omega0 = 1.

for i in range(10000):
    if tau < dtau:
        f = 0.
        tau += dt
    else:
        tau = 0.
        f = random.gauss(0,f_std)

    result = step(x, dt, gamma = gamma, omega0 = omega0, f=f)
    position += [result[0,0]]
    velosity += [result[1,0]]
    time += [dt * i]
In [6]:
plt.figure(figsize=(18,8))
plt.plot(time, position)
plt.grid()

Фильтр Калмана

Размер матриц

x [k,1]

F [k,k]

Q [k,k]

P [k,k]

y [m,1]

H [m,k]

R [m,m]

In [7]:
gamma = 0.05
omega0 = 1.
# Вводится слоучайное отличие к константы модели
omega_st = 0.2
gamma_st = 0.01
omega0_ = random.gauss(omega0,omega_st)
omega0_ = random.gauss(gamma,gamma_st)

F = np.array([[1., dt],[- omega0_**2* dt,(1 - 2 * gamma * dt) ]])

Q = np.array([[1., 0.],[0., 1.]])
H = np.array([[1., 0.]])
R = np.array([[1.0]])
In [8]:
gamma = 0.05
omega0 = 1.

x = np.array([[1.],[0.]])
x_ = np.array([[1.],[0.]])
P = np.array([[999.,0.],[999.,0.]])

dt = 0.005
dtau = 0.004
f_std = 10.

position = [x[0,0]]
velosity = [x[1,0]]
position_ = [x_[0,0]]
velosity_ = [x_[1,0]]
predict_position = [x_[0,0]]
predict_velosity = [x_[1,0]]
time = [0]
tau = 0.
for i in range(10000):
    if tau < dtau:
        f = 0.
        tau += dt
    else:
        tau = 0.
        f = random.gauss(0,f_std)

    x = step(x, dt, gamma = gamma, omega0 = omega0, f=f)
    position += [x[0,0]]
    velosity += [x[1,0]]

    # Этап экстраполяции
    x_ = np.dot(F,x_)
    predict_position += [x_[0,0]]
    predict_velosity += [x_[1,0]]
    P =  np.dot(np.dot(F,P),F.transpose()) + Q
    
    # Этап коррекции
    # Размер матрицы [m,1]
    z = [[x[0,0]]]
    
    # Размер матрицы [m,k][k,1]
    y = z - np.dot(H,x_)

    # Размер матрицы [m,k][k,k][k,m]
    S = np.dot(np.dot(H,P),H.transpose()) + R
    
    K = np.dot(np.dot(P,H.transpose()),np.linalg.inv(S))

    x_ = x_ + np.dot(K,y)
        
    aaa = (np.eye(len(P)) - np.dot(K,H))
       
    P = np.dot(aaa,P)
 
    position_ += [x_[0,0]]
    velosity_ += [x_[1,0]]

    time += [dt * i]

На графике ниже показана зависимость реального отклонения осциллятора - зеленые кружки, которые слились в полосу.

Отклонение осциллятора, оцененное фильтром Калмана - желтая линия в середине зеленой полосы.

Отклонение осциллятора, оцененное фильтром Калмана на этапе экстраполяции - малиновый пунктир, расположенный практически на желтой линии.

In [9]:
plt.figure(figsize=(18,8))
plt.plot(time, position,'go')
plt.plot(time, position_, 'y')
plt.plot(time, predict_position,'m--')
plt.grid()

На графике ниже показана разность между реальным отклонением осциллятора и оценкой фильтром Калмана - зеленый пунктир.

Красные точки - разность между реальным отклонением осциллятора и оценкой фильтром Калмана на этапе экстраполяции

In [10]:
plt.figure(figsize=(18,8))
plt.plot(time, np.array(position) - np.array(predict_position),'r.')
plt.plot(time, np.array(position) - np.array(position_),'g--')
plt.grid()

На графике ниже показана зависимость реальной скорости осциллятора - зеленые кружки, которые слились в полосу.

Скорость осциллятора, оцененное фильтром Калмана - желтые кружки, которые слились в полосу.

Скорость осциллятора, оцененное фильтром Калмана на этапе экстраполяции - малиновый пунктир, расположенный практически на желтой полосе.

NB Замер скорости осциллятора не подавался в фильтр Калмана

In [11]:
plt.figure(figsize=(18,8))
plt.plot(time, velosity,'go')
plt.plot(time, velosity_,'yo')
plt.plot(time, predict_velosity,'m--')
plt.grid()

На графике ниже показана разность между реальной скоростью осциллятора и оценкой фильтром Калмана - зеленый пунктир.

Красные точки - разность между реальной скоростью осциллятора и оценкой фильтром Калмана на этапе экстраполяции

In [12]:
plt.figure(figsize=(18,8))
plt.plot(time, np.array(velosity) - np.array(predict_velosity),'r.')
plt.plot(time, np.array(velosity) - np.array(velosity_),'g--')
plt.grid()

Вернуться на страницу brain2net:

https://brain2net.ru/post/model-prostogo-osczillyatora/#more-67

In [ ]: