-
Notifications
You must be signed in to change notification settings - Fork 19
/
Copy pathrecursive_least_squares.py
132 lines (111 loc) · 3.09 KB
/
recursive_least_squares.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
"""
Example recursive_least_squares.py
Author: Joshua A. Marshall <[email protected]>
GitHub: https://github.com/botprof/agv-examples
"""
# %%
# SIMULATION SETUP
import numpy as np
from numpy.linalg import inv
import matplotlib.pyplot as plt
from scipy.stats import chi2
# Set the simulation time [s] and the sample period [s]
SIM_TIME = 2.0
T = 0.1
# Create an array of time values [s]
t = np.arange(0.0, SIM_TIME, T)
N = np.size(t)
# %%
# FUNCTION DEFINITIONS
def gps_sensor(x, R, H):
"""Simulated GPS sensor."""
y = H @ x + R @ np.random.randn(np.size(x))
return y
def rls_estimator(x, P, y, R, H):
n = np.size(x)
K = P @ H.T @ inv(H @ P @ H.T + R)
x_next = x + K @ (y - H @ x)
P_next = (np.eye(n) - K @ H) @ P @ (np.eye(n) - K @ H).T + K @ R @ K.T
return x_next, P_next
# %%
# SET UP INITIAL CONDITIONS
# Set up arrays for a two-dimensional GPS measurements problem
x_hat = np.zeros((2, N))
P_hat = np.zeros((2, 2, N))
y = np.zeros((2, N))
# Robot location (actual)
x = np.array([2.0, 4.0])
# Initial estimate of the robot's location
x_hat[:, 0] = np.array([0.0, 0.0])
# Initial covariance matrix
P_hat[:, :, 0] = 10.0 * np.eye(2)
# Measurement noise covariance matrix
SIGMA = 1.0
R = SIGMA**2 * np.eye(2)
# Measurement matrix (i.e., GPS coordinates)
H = np.eye(2)
# %%
# RUN THE SIMULATION
for k in range(1, N):
# Simulate the GPS sensor
y[:, k] = gps_sensor(x, R, H)
# Update the estimate using the recursive least squares estimator
x_hat[:, k], P_hat[:, :, k] = rls_estimator(
x_hat[:, k - 1], P_hat[:, :, k - 1], y[:, k], R, H
)
# %%
# PLOT THE RESULTS
# Change some plot settings (optional)
plt.rc("text", usetex=True)
plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}")
plt.rc("savefig", format="pdf")
plt.rc("savefig", bbox="tight")
# Plot the true and estimated robot location
plt.figure()
plt.plot(x[0], x[1], "C4o", label="True")
plt.plot(x_hat[0, :], x_hat[1, :], "C0-", label="Estimated")
plt.xlabel(r"$x_1$ [m]")
plt.ylabel(r"$x_2$ [m]")
plt.legend()
plt.grid(color="0.95")
plt.axis("equal")
# Find the scaling factor for plotting covariance bounds
ALPHA = 0.01
s1 = chi2.isf(ALPHA, 1)
s2 = chi2.isf(ALPHA, 2)
# Plot the error in the estimate
sigma = np.zeros((2, N))
plt.figure()
ax1 = plt.subplot(211)
sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :])
plt.fill_between(
t,
-sigma[0, :],
sigma[0, :],
color="C0",
alpha=0.2,
label=str(100 * (1 - ALPHA)) + r" \% Confidence",
)
plt.plot(t, x[0] - x_hat[0, :], "C0", label="Estimator Error")
plt.plot(t, x[0] - y[0, :], "C1", label="Raw Measurement Error")
plt.ylabel(r"$e_1$ [m]")
plt.setp(ax1, xticklabels=[])
plt.grid(color="0.95")
plt.legend()
ax2 = plt.subplot(212)
sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :])
plt.fill_between(
t,
-sigma[1, :],
sigma[1, :],
color="C0",
alpha=0.2,
label=str(100 * (1 - ALPHA)) + r" \% Confidence",
)
plt.plot(t, x[1] - x_hat[1, :], "C0", label="Estimator Error")
plt.plot(t, x[1] - y[1, :], "C1", label="Raw Measurement Error")
plt.ylabel(r"$e_2$ [m]")
plt.grid(color="0.95")
plt.xlabel(r"$t$ [s]")
plt.show()
# %%