Skip to content
Snippets Groups Projects
Commit 77a2ed91 authored by Erik Frisk's avatar Erik Frisk
Browse files

fixed issue with code

parent f5cd994c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Exercise/EKF_UKF/task1.ipynb
+ 2
− 2
View file @ 77a2ed91
%% Cell type:code id: tags:
``` python
import numpy as np
import matplotlib.pyplot as plt
import seaborn as sns
from seaborn import despine
import pandas as pd
sns.set(style='white', rc={'lines.linewidth': 0.75})
```
%% Cell type:markdown id: tags:
In case you want your plots in external windows
%% Cell type:code id: tags:
``` python
# %matplotlib
```
%% Cell type:markdown id: tags:
# Simulate robot movement
%% Cell type:markdown id: tags:
Define discrete-time model
%% Cell type:code id: tags:
``` python
Tfinal = 20
Ts = 0.1
V = 3
x0 = [10, 10, 0] # Initial state
model = {}
model['f'] = lambda x, u, w: [x[0] + Ts*V*np.cos(x[2]), x[1] + Ts*V*np.sin(x[2]), x[2] + Ts*u]
model['f'] = lambda x, u, w: [x[0] + Ts * V * np.cos(x[2]), x[1] + Ts * V * np.sin(x[2]), x[2] + Ts*u[0]]
model['h'] = lambda x, u: np.array([np.sqrt(x[0]**2 + x[1]**2), np.arctan2(x[1], x[0])])
```
%% Cell type:markdown id: tags:
Simulate robot movement and store results in a dictionary
%% Cell type:code id: tags:
``` python
t = np.arange(0, Tfinal, Ts)
N = len(t)
x = np.zeros((3, N))
x[:, 0] = x0
u = np.zeros((1, N))
u[0][(t >= 5) & (t < 10)] = -1
u[0][(t >= 10) & (t < 12)] = 0
u[0][(t >= 12) & (t < 15)] = 1
u[0][(t >= 15) & (t < 17)] = -1
for ti in np.arange(0, N-1):
x[:, ti + 1] = model['f'](x[:, ti], u[0, ti], 0)
x[:, ti + 1] = model['f'](x[:, ti], u[:, ti], 0)
y0 = model['h'](x, u)
y = y0 + (np.diag([0.6, 0.03]) @ np.random.normal(0, 1, size=(2, N)))
data = {'t': t, 'x': x, 'u': u, 'y0': y0, 'y': y}
```
%% Cell type:markdown id: tags:
Plot nominal path
%% Cell type:code id: tags:
``` python
_, ax = plt.subplots(num=10, clear=True)
ax.plot(data['x'][0, :], data['x'][1, :])
ax.plot(data['x'][0, 0], data['x'][1, 0], 'bo')
ax.plot(data['x'][0, -1], data['x'][1, -1], 'rx')
ax.axis('square')
ax.set_xlabel('x')
ax.set_ylabel('y')
despine(ax=ax)
```
%% Cell type:markdown id: tags:
Plot measurements
%% Cell type:code id: tags:
``` python
_, ax = plt.subplots(2, 1, num=20, clear=True, layout="tight")
ax[0].plot(data['t'], data['y0'][0], 'b', lw=2)
ax[0].plot(data['t'], data['y'][0], 'r')
ax[0].set_xlabel('t [s]')
ax[0].set_ylabel('[m]')
ax[0].set_title('Range measurement')
despine(ax=ax[0])
ax[1].plot(data['t'], data['y0'][1]*180/np.pi, 'b', lw=2)
ax[1].plot(data['t'], data['y'][1]*180/np.pi, 'r')
ax[1].set_xlabel('t [s]')
ax[1].set_ylabel('[deg]')
ax[1].set_title('Angle measurement')
despine(ax=ax[1])
```
%% Cell type:markdown id: tags:
# Plot path computed from direct measurement
%% Cell type:markdown id: tags:
Compute position estimate by inverting the measurement equation
%% Cell type:code id: tags:
``` python
dist = data['y'][0]
ang = data['y'][1]
pos_hat_0 = np.row_stack((dist*np.cos(ang), dist*np.sin(ang)))
```
%% Cell type:markdown id: tags:
Plot the estimated path
%% Cell type:code id: tags:
``` python
_, ax = plt.subplots(num=30, clear=True)
ax.plot(data['x'][0], data['x'][1])
ax.plot(data['x'][0, 0], data['x'][1, 0], 'bo')
ax.plot(data['x'][0, -1], data['x'][1, -1], 'rx')
ax.plot(pos_hat_0[0], pos_hat_0[1], 'k')
ax.axis('square')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_title('Direct computation from measurements')
despine(ax=ax)
```
%% Cell type:markdown id: tags:
Plot position errors
%% Cell type:code id: tags:
``` python
_, ax = plt.subplots(2, 1, num=31, clear=True, layout="tight")
ax[0].plot(data['t'], data['x'][0] - pos_hat_0[0])
ax[0].set_xlabel('t [s]');
ax[0].set_ylabel('[m]')
ax[0].set_title('x position error in direct computation');
despine(ax=ax[0])
ax[1].plot(data['t'], data['x'][1] - pos_hat_0[1])
ax[1].set_xlabel('t [s]');
ax[1].set_ylabel('[m]')
ax[1].set_title('y position error in direct computation')
despine(ax=ax[1])
```
%% Cell type:markdown id: tags:
# Extended Kalman Filter
%% Cell type:markdown id: tags:
Implement your Extended Kalman Filter below
%% Cell type:code id: tags:
``` python
def EKF(model, init, data):
"""Extended Kalman Filter
Input:
model - Dictionary with keys n, f, h, fx, hx, Q, R
init - Dictionary with x0, P0
"""
M = data['y'].shape[1] # Number of data points
P = init['P0'] # Initial covariance P_0|-1
n = model['n'] # Number of states
xhat = np.zeros((n, M)) # Prepare output array
xhat[:, 0] = init['x0'] # Initialize xhat
for l in range(0, M - 1):
pass
# Measurement update
# FILL IN YOUR CODE HERE
# Time update
# FILL IN YOUR CODE HERE
return xhat
```
%% Cell type:markdown id: tags:
Define model dictionary
%% Cell type:code id: tags:
``` python
modelEKF = {}
Q = None # YOUR CODE HERE: 3x3 numpy array matrix, Process noise covariance
R = None # YOUR CODE HERE: 2x2 numpy array matrix, Measurement noise covariance
modelEKF['n'] = 3
modelEKF['m'] = 2
modelEKF['Q'] = lambda x: Q
modelEKF['R'] = lambda x: R
modelEKF['f'] = lambda x, u: model['f'](x, u, 0)
modelEKF['h'] = lambda x, u: model['h'](x,u)
modelEKF['fx'] = lambda x, u: None # YOUR CODE HERE: return df/dx(x, u)
modelEKF['hx'] = lambda x, u: None # YOUR CODE HERE: return dh/dx(x, u)
```
%% Cell type:markdown id: tags:
Define inital conditions and run filter
%% Cell type:code id: tags:
``` python
init = {}
P0 = None # YOUR CODE HERE: 2x2 numpy array matrix, Initial estimate covariance
x0 = None # YOUR CODE HERE: numpy array with initial state (x0, y0, theta0)
init['x0'] = x0
init['P0'] = P0
xhatEKF = EKF(modelEKF, init, data)
```
%% Cell type:markdown id: tags:
Explore, investigate, and plot interesting results.
%% Cell type:code id: tags:
``` python
```
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment