Gyakorlat¶
Import¶
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
%matplotlib inline
plt.rcParams["figure.figsize"] = [16, 9]
plt.rc("xtick", labelsize=20)
plt.rc("ytick", labelsize=20)
Kalman Filter for Constant Acceleration Model¶
State Vector - Constant Acceleration¶
Constant Acceleration Model for Ego Motion in Plane
\[x_{k+1} = A \cdot x_{k} + B \cdot u\]
Newton's Equations of Motion¶
$$ x = x_0 + v_{x0} * t + \frac{1}{2}a_x * t^2 $$
\[ y = y_0 + v_{y0} * t + \frac{1}{2}a_y * t^2 \]
State Transition (no control input)¶
\[x_{k+1} = \begin{bmatrix}1 & 0 & \Delta t & 0 & \frac{1}{2}\Delta t^2 & 0 \\ 0 & 1 & 0 & \Delta t & 0 & \frac{1}{2}\Delta t^2 \\ 0 & 0 & 1 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 1 & 0 & \Delta t \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1\end{bmatrix} \cdot \begin{bmatrix} x \\ y \\ \dot x \\ \dot y \\ \ddot x \\ \ddot y\end{bmatrix}_{k}\]
\[y = H \cdot x\]
Acceleration (IMU) and position (GNSS) (\(\ddot x\), \(\ddot y\), \(x\), \(y\)) sensors are used.
\[y = \begin{bmatrix}0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \end{bmatrix} \cdot x\]
Initial State¶
plt.figure(figsize=(18, 10))
x = np.matrix([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T
n = x.size # States
plt.scatter(float(x[0]), float(x[1]), s=100)
plt.title("Initial Location", fontsize=20)
plt.ylabel("y in m", fontsize=20)
plt.xlabel("x in m", fontsize=20)
plt.grid(True)
#### Initial Uncertainty
P = np.matrix(
[
[10.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 10.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 10.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 10.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 10.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 10.0],
]
)
print(P)
fig = plt.figure(figsize=(6, 6))
im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap("binary"))
plt.title("Initial Covariance Matrix $P$")
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(7))
# set the locations and labels of the yticks
plt.yticks(
np.arange(6),
("$x$", "$y$", "$\dot x$", "$\dot y$", "$\ddot x$", "$\ddot y$"),
fontsize=22,
)
xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(7))
# set the locations and labels of the yticks
plt.xticks(
np.arange(6),
("$x$", "$y$", "$\dot x$", "$\dot y$", "$\ddot x$", "$\ddot y$"),
fontsize=22,
)
plt.xlim([-0.5, 5.5])
plt.ylim([5.5, -0.5])
from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())
cax = divider.append_axes("right", "5%", pad="3%")
plt.colorbar(im, cax=cax)
plt.tight_layout()
Dynamic Matrix¶
It is calculated from the dynamics of the Egomotion.
\(\(x_{k+1} = x_{k} + \dot x_{k} \cdot \Delta t + \ddot x_k \cdot \frac{1}{2}\Delta t^2\)\)
\[y_{k+1} = y_{k} + \dot y_{k} \cdot \Delta t + \ddot y_k \cdot \frac{1}{2}\Delta t^2\]
\(\(\dot x_{k+1} = \dot x_{k} + \ddot x \cdot \Delta t\)\)
\[\dot y_{k+1} = \dot y_{k} + \ddot y \cdot \Delta t\]
\(\(\ddot x_{k+1} = \ddot x_{k}\)\)
\[\ddot y_{k+1} = \ddot y_{k}\]
dt = 0.1 # Time Step between Filter Steps
A = np.matrix(
[
[1.0, 0.0, dt, 0.0, 1 / 2.0 * dt ** 2, 0.0],
[0.0, 1.0, 0.0, dt, 0.0, 1 / 2.0 * dt ** 2],
[0.0, 0.0, 1.0, 0.0, dt, 0.0],
[0.0, 0.0, 0.0, 1.0, 0.0, dt],
[0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
]
)
print(A)
## Measurement Matrix
This matrix determines how the sensor measurements map to the vehicle state. In this example, the position and the accelerations are measured ($x$, $y$, $\ddot x$, $\ddot y$).
H = np.matrix(
[
[0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0, 0.0, 0.0],
]
)
print(H)
## Measurement Noise Covariance
ra = 10.0 ** 2
rp = 2.0 ** 2
R = np.matrix(
[[ra, 0.0, 0.0, 0.0], [0.0, ra, 0.0, 0.0], [0.0, 0.0, rp, 0.0], [0.0, 0.0, 0.0, rp]]
)
print(R)
Process Noise Covariance Matrix Q for CA Model¶
The Position of an object can be influenced by a force (e.g. wind), which leads to an acceleration disturbance (noise). This process noise has to be modeled with the process noise covariance matrix Q.
\[Q = \begin{bmatrix}
\sigma_{x}^2 & 0 & \sigma_{x \dot x} & 0 & \sigma_{x \ddot x} & 0 \\
0 & \sigma_{y}^2 & 0 & \sigma_{y \dot y} & 0 & \sigma_{y \ddot y} \\
\sigma_{\dot x x} & 0 & \sigma_{\dot x}^2 & 0 & \sigma_{\dot x \ddot x} & 0 \\
0 & \sigma_{\dot y y} & 0 & \sigma_{\dot y}^2 & 0 & \sigma_{\dot y \ddot y} \\
\sigma_{\ddot x x} & 0 & \sigma_{\ddot x \dot x} & 0 & \sigma_{\ddot x}^2 & 0 \\
0 & \sigma_{\ddot y y} & 0 & \sigma_{\ddot y \dot y} & 0 & \sigma_{\ddot y}^2
\end{bmatrix} \cdot \sigma_{j}\]
Symbolic Calculation¶
from sympy import Symbol, Matrix
from sympy.interactive import printing
dts = Symbol("\Delta t")
sj = 0.1
Q = (
np.matrix(
[
[(dt ** 6) / 36, 0, (dt ** 5) / 12, 0, (dt ** 4) / 6, 0],
[0, (dt ** 6) / 36, 0, (dt ** 5) / 12, 0, (dt ** 4) / 6],
[(dt ** 5) / 12, 0, (dt ** 4) / 4, 0, (dt ** 3) / 2, 0],
[0, (dt ** 5) / 12, 0, (dt ** 4) / 4, 0, (dt ** 3) / 2],
[(dt ** 4) / 6, 0, (dt ** 3) / 2, 0, (dt ** 2), 0],
[0, (dt ** 4) / 6, 0, (dt ** 3) / 2, 0, (dt ** 2)],
]
)
* sj ** 2
)
print(Q)
fig = plt.figure(figsize=(6, 6))
im = plt.imshow(Q, interpolation="none", cmap=plt.get_cmap("binary"))
plt.title("Process Noise Covariance Matrix $Q$")
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(7))
# set the locations and labels of the yticks
plt.yticks(
np.arange(6),
("$x$", "$y$", "$\dot x$", "$\dot y$", "$\ddot x$", "$\ddot y$"),
fontsize=22,
)
xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(7))
# set the locations and labels of the yticks
plt.xticks(
np.arange(6),
("$x$", "$y$", "$\dot x$", "$\dot y$", "$\ddot x$", "$\ddot y$"),
fontsize=22,
)
plt.xlim([-0.5, 5.5])
plt.ylim([5.5, -0.5])
from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())
cax = divider.append_axes("right", "5%", pad="3%")
plt.colorbar(im, cax=cax)
plt.tight_layout()
## Identity Matrix
I = np.eye(n)
## Measurement
import pandas as pd
from pyproj import Proj
# Read data
# Use every 5th value to get GPS updates in every timestep
n_rows = 10800 # len(df)
skip = np.arange(n_rows)
skip = np.delete(skip, np.arange(0, n_rows, 5))
df = pd.read_csv("data/2014-03-26-000-Data.csv", skiprows=skip)
# Extract values
ax = df["ax"].dropna()
ay = df["ay"].dropna()
px = df["latitude"].dropna()
py = df["longitude"].dropna()
m = len(df["ax"]) # Measurements
# Lat Lon to UTM
utm_converter = Proj(
"+proj=utm +zone=33U, +south +ellps=WGS84 +datum=WGS84 +units=m +no_defs"
)
for i in range(len(px)):
py[i], px[i] = utm_converter(py[i], px[i])
px[i] = px[i] + np.random.normal(0, 2.0, 1)
py[i] = py[i] + np.random.normal(0, 2.0, 1)
# px[i] = 0 #TODO
# py[i] = 0 #TODO
# Start from position (0 ,0)
py_offset = py[0]
px_offset = px[0]
px = px - px_offset
py = py - py_offset
# Stack measurement vector
measurements = np.vstack((ax, ay, px, py))
fig = plt.figure(figsize=(16, 9))
plt.step(range(m), ax, label="$a_x$")
plt.step(range(m), ay, label="$a_y$")
plt.ylabel("Acceleration in m / $s^2$", fontsize=20)
plt.xlabel("Number of measurements", fontsize=20)
plt.title("IMU Measurements", fontsize=20)
plt.ylim([-20, 20])
plt.legend(loc="best", prop={"size": 18})
fig = plt.figure(figsize=(16, 9))
plt.step(px, py, label="$GNSS$")
plt.xlabel("x in m", fontsize=20)
plt.ylabel("y in m", fontsize=20)
plt.title("GNSS Measurements", fontsize=20)
plt.xlim([min(px), max(px)])
plt.ylim([min(py), max(py)])
plt.legend(loc="best", prop={"size": 18})
# Preallocation for Plotting
xt = []
yt = []
dxt = []
dyt = []
ddxt = []
ddyt = []
Zx = []
Zy = []
Px = []
Py = []
Pdx = []
Pdy = []
Pddx = []
Pddy = []
Kx = []
Ky = []
Kdx = []
Kdy = []
Kddx = []
Kddy = []
Kalman Filter¶
for n in range(m):
# Time Update (Prediction)
# ========================
# Project the state ahead
x = A * x
# Project the error covariance ahead
P = A * P * A.T + Q
# Measurement Update (Correction)
# ===============================
# Compute the Kalman Gain
S = H * P * H.T + R
K = (P * H.T) * np.linalg.pinv(S)
# Update the estimate via z
Z = measurements[:, n].reshape(H.shape[0], 1)
y = Z - (H * x) # Innovation or Residual
x = x + (K * y)
# Update the error covariance
P = (I - (K * H)) * P
# Save states for Plotting
xt.append(float(x[0]))
yt.append(float(x[1]))
dxt.append(float(x[2]))
dyt.append(float(x[3]))
ddxt.append(float(x[4]))
ddyt.append(float(x[5]))
Zx.append(float(Z[0]))
Zy.append(float(Z[1]))
Px.append(float(P[0, 0]))
Py.append(float(P[1, 1]))
Pdx.append(float(P[2, 2]))
Pdy.append(float(P[3, 3]))
Pddx.append(float(P[4, 4]))
Pddy.append(float(P[5, 5]))
Kx.append(float(K[0, 0]))
Ky.append(float(K[1, 0]))
Kdx.append(float(K[2, 0]))
Kdy.append(float(K[3, 0]))
Kddx.append(float(K[4, 0]))
Kddy.append(float(K[5, 0]))
## Plots
### Covariance Matrix
fig = plt.figure(figsize=(6, 6))
im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap("binary"))
plt.title("Covariance Matrix $P$ (after %i Filter Steps)" % (m), fontsize=20)
ylocs, ylabels = plt.yticks()
# set the locations of the yticks
plt.yticks(np.arange(7))
# set the locations and labels of the yticks
plt.yticks(
np.arange(6),
("$x$", "$y$", "$\dot x$", "$\dot y$", "$\ddot x$", "$\ddot y$"),
fontsize=22,
)
xlocs, xlabels = plt.xticks()
# set the locations of the yticks
plt.xticks(np.arange(7))
# set the locations and labels of the yticks
plt.xticks(
np.arange(6),
("$x$", "$y$", "$\dot x$", "$\dot y$", "$\ddot x$", "$\ddot y$"),
fontsize=22,
)
plt.xlim([-0.5, 5.5])
plt.ylim([5.5, -0.5])
from mpl_toolkits.axes_grid1 import make_axes_locatable
divider = make_axes_locatable(plt.gca())
cax = divider.append_axes("right", "5%", pad="3%")
plt.colorbar(im, cax=cax)
plt.tight_layout()
## State Vector
fig = plt.figure(figsize=(16, 12))
plt.subplot(311)
plt.step(range(len(measurements[0])), ddxt, label="$\ddot x$")
plt.step(range(len(measurements[0])), ddyt, label="$\ddot y$")
plt.title("Estimate (Elements from State Vector $x$)", fontsize=20)
plt.legend(loc="best", prop={"size": 22})
plt.ylabel("Acceleration in m/ $s^2$", fontsize=20)
plt.subplot(312)
plt.step(range(len(measurements[0])), dxt, label="$\dot x$")
plt.step(range(len(measurements[0])), dyt, label="$\dot y$")
plt.ylabel("")
plt.legend(loc="best", prop={"size": 22})
plt.ylabel("Velocity in m/s", fontsize=20)
plt.subplot(313)
plt.step(range(len(measurements[0])), xt, label="$x$")
plt.step(range(len(measurements[0])), yt, label="$y$")
plt.xlabel("Filter Step", fontsize=20)
plt.ylabel("Position in m", fontsize=20)
plt.legend(loc="best", prop={"size": 22})
## Position x/y
fig = plt.figure(figsize=(16, 9))
plt.step(px, py, label="$GNSS$")
plt.scatter(xt[0], yt[0], s=100, label="Start", c="g")
plt.scatter(xt[-1], yt[-1], s=100, label="Goal", c="r")
plt.plot(xt, yt, label="State", alpha=0.5)
plt.xlabel("x in m", fontsize=20)
plt.ylabel("y in m", fontsize=20)
plt.title("Position", fontsize=20)
plt.legend(loc="best", fontsize=20)
plt.xlim(min(xt), max(xt))
plt.ylim(min(yt), max(yt))