# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 3 (ipykernel)
#     language: python
#     name: python3
# ---

# # Home assignment #1. Kalman Filter

# Imagine a robot. Robot state is described with the following parameters
#
# 1. $x$, $y$ - robot coordinates,
# 2. $V_x$, $V_y$ - velocities.
#
# We can only measure the coordinates of the robot, which should be reflected in the $ H $ matrix.
#
# In this homework assignment:
# - Fill in the matrices for the Kalman filter correctly.
# - For all three experiments, visualize the dependence of $ x $, $ y $, $ V_x $, $ V_y $ over time.
# - For all three experiments, visualize the dependence of each component of the gain matrix ($ K $) over time.
#      - What does the dynamics of changes in its components say?
# - How much does the velocity uncertainty decrease as a result of each experiment?

# +
import numpy as np
import matplotlib.pyplot as plt

from filterpy.stats import plot_covariance_ellipse


# +
def filter(x, E): #В python есть встроенная функция filter, лучше переименовать
    x_list = []
    E_list = []
    k_list = []
    
    for Z in measurements:
        # prediction
        x = (F * x) + u
        E = F * E * F.transpose()

        # measurement update
        Z = np.matrix(Z)
        S = H * E * H.transpose() + R
        K = E * H.transpose() * np.linalg.inv(S)
        x = x + (K * (Z.transpose() - (H * x)))
        E = (I - (K * H)) * E
        
        x_list.append(x)
        E_list.append(E)
        k_list.append(K)
        
        print("x= ")
        print(x)
        print("E= ")
        print(E)
        
    return x_list, E_list, k_list

def plot_position_xy(x, E, edgecolor='r'):
    xy= np.array([x[0, 0],x[1, 0]])
    E_xy= np.array([[E[0, 0],E[0, 1]],[E[1, 0],E[1, 1]]])
    plot_covariance_ellipse(xy, E_xy, edgecolor=edgecolor)

def plot_position_velocity(x, E, edgecolor='r'):
    xy= np.array([x[2, 0],x[3, 0]])
    E_xy= np.array([[E[2, 2],E[2, 3]],[E[3, 2],E[3, 3]]])
    plot_covariance_ellipse(xy, E_xy, edgecolor=edgecolor)    
    

def plot_position_xy_and_velocity(x, E, x_list, E_list):
    gridsize = (1, 2)
    fig = plt.figure(figsize=(16, 8))
    
    plt.subplot2grid(gridsize, (0, 0))
    plt.title("Изменение координат со временем с дисперсией")
    plt.xlabel("x")
    plt.ylabel("y")
    plot_position_xy(x, E)
    for i in range(len(x_list)):
        plot_position_xy(x_list[i], E_list[i], edgecolor='g')
        
    plt.subplot2grid(gridsize, (0, 1))
    plt.title("Изменение скорости со временем с дисперсией")
    plt.xlabel("Vx")
    plt.ylabel("Vy")
    plot_position_velocity(x, E)
    for i in range(len(x_list)):
        plot_position_velocity(x_list[i], E_list[i], edgecolor='g')

def plot_components_k(k_list):        
    gridsize = k_list[0].shape
    fig = plt.figure(figsize=(6 * k_list[0].shape[1], 6 * k_list[0].shape[0]))
    
    for i in range(k_list[0].shape[0]):
        for j in range(k_list[0].shape[1]):
            plt.subplot2grid(gridsize, (i, j))
            plt.title("k[" + str(i) + "][" + str(j) + "]")
            plt.xlabel("Время")
            plt.ylabel("Значение компоненты")
            plt.plot(np.array(k_list)[:,i, j], color="b")
        


# +
dt = 0.1

# initial covariance matrix: 0. for positions x and y, 1000 for the two velocities
E = np.matrix([
    [0., 0., 0., 0.],
    [0., 0., 0., 0.],
    [0., 0., 1000., 0.],
    [0., 0., 0., 1000.]
])

# next state function: 4D
F = np.matrix([
    [1., 0., dt, 0.], 
    [0., 1., 0., dt],
    [0., 0., 1., 0.],
    [0., 0., 0., 1.]
])

# measurement function: reflect the fact that we observe x and y but not the two velocities
H = np.matrix([
    [1., 0., 0., 0.],
    [0., 1., 0., 0.]
])
#R = # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
R = np.matrix([
    [0.1, 0.],
    [0., 0.1]
])

#I = # 4D identity matrix
I = np.matrix([
    [1., 0., 0., 0.], 
    [0., 1., 0., 0.],
    [0., 0., 1., 0.],
    [0., 0., 0., 1.]
])


# +
print("### First Experiment ###")
measurements = [
    [5.0, 10.0], 
    [6.0, 8.0], 
    [7.0, 6.0], 
    [8.0, 4.0], 
    [9.0, 2.0], 
    [10.0, 0.0]
]

initial_xy = [4.0, 12.0]

# initial robot state (location and velocity)
x = np.matrix([[initial_xy[0]], [initial_xy[1]], [0.0], [0.0]])

# external motion applied to the robot
u = np.matrix([[0.0], [0.1], [0.0], [0.0]])
# -

x_list, E_list, k_list = filter(x, E)

# Visualize the dependence of $ x $, $ y $, $ V_x $, $ V_y $ over time

# YOUR CODE HERE
plot_position_xy_and_velocity(x, E, x_list, E_list)

# Visualize the components of the $ K $ matrix below

# YOUR CODE HERE
plot_components_k(k_list)

# +
print("### Second Experiment ###")
measurements = [
    [1.0, 4.0],
    [6.0, 0.0], 
    [11.0, -4.0], 
    [16.0, -8.0]
]

initial_xy = [-4.0, 8.0]

dt = 0.1

# initial robot state (location and velocity)
x = np.matrix([[initial_xy[0]], [initial_xy[1]], [0.0], [0.0]])
# external motion applied to the robot
u = np.matrix([[0.0], [0.1], [0.0], [0.0]])
# -

x_list, E_list, k_list = filter(x, E)

# Visualize the dependence of $ x $, $ y $, $ V_x $, $ V_y $ over time

# YOUR CODE HERE
plot_position_xy_and_velocity(x, E, x_list, E_list)

# Visualize the components of the $ K $ matrix below

# YOUR CODE HERE
plot_components_k(k_list)

# +
print("### Third Experiment ###")
measurements = [
    [1.0, 17.0], 
    [1.0, 15.0], 
    [1.0, 13.0], 
    [1.0, 11.0]
]

initial_xy = [1.0, 19.0]

dt = 0.1
# initial robot state (location and velocity)
x = np.matrix([[initial_xy[0]], [initial_xy[1]], [0.0], [0.0]])  
# external motion applied to the robot
u = np.matrix([[0.0], [0.1], [0.0], [0.0]])  
# -

x_list, E_list, k_list = filter(x, E)

# Visualize the dependence of $ x $, $ y $, $ V_x $, $ V_y $ over time

# YOUR CODE HERE
plot_position_xy_and_velocity(x, E, x_list, E_list)

# Visualize the components of the $ K $ matrix below

# YOUR CODE HERE
plot_components_k(k_list)

# +
print("### Fourth Experiment ###")
measurements = []

for x in range(1, 30, 1):
    measurements.append([x*0.5, np.sin(x*0.5)])
initial_xy = [0.0, 0.0]

dt = 0.1

# initial robot state (location and velocity)
x = np.matrix([[initial_xy[0]], [initial_xy[1]], [0.0], [0.0]])  
# external motion applied to the robot
u = np.matrix([[0.0], [0.1], [0.0], [0.0]])  
# -

x_list, E_list, k_list = filter(x, E)

plot_position_xy_and_velocity(x, E, x_list, E_list)

plot_components_k(k_list)

# +
print("### Fifth Experiment ###")
measurements = []

for alpha in range(1, 31, 1):
    alpha *= np.pi / 30
    measurements.append([np.cos(alpha), np.sin(alpha)])
initial_xy = [1.0, 0.0]

dt = 0.1

# initial robot state (location and velocity)
x = np.matrix([[initial_xy[0]], [initial_xy[1]], [0.0], [0.0]])  
# external motion applied to the robot
u = np.matrix([[0.0], [0.1], [0.0], [0.0]])   
# -

x_list, E_list, k_list = filter(x, E)

plot_position_xy_and_velocity(x, E, x_list, E_list)

plot_components_k(k_list)

# #### Компоненты уменьшаются, похоже экспоненциально, неопределенность уменьшается. Скорость стремиться к константе. Так же уменьшается дисперсия. 
#
# #### Для равномерного прямолинейного движения результаты хорошие. 
#
# #### В случае неравномерного движения (предпоследний эксперимент) так же все затухает, поэтому контролировать скорость невозможно
#
# #### В случае равномерного непрямолинейного  (последний эксперимент) так же все затухает, поэтому контролировать угол вектора скорости невозможно, т.е. I закон Ньютона все таки не раотает для этой модели

StarcoderdataJupyterScriptsDedupFiltered