Kalman Filter
0. Objective
Implement a Kalman Filter, which will helps execute the behavior in Lab 6 fast.
Task A
1. Step Response
I set the duty cycle to 90
, then put the robot at the 1.6m distance from the wall.
The max speed is around 1.7 m/s
, therefore
2. Kalman Filter Setup
Let sigma_1 = 10, sigma_2 = 10, sigma_3 = 20
, then the process noise and the sensor noise covariance matrices are
Since I am doing Task A, the C matrix would be C = np.array([[-1,0]])
.
1
2
3
4
5
6
sigma_1 = 10;
sigma_2 = 10;
sigma_3 = 20;
sig_u=np.array([[sigma_1**2,0],[0,sigma_2**2]]) //We assume uncorrelated noise, therefore a diagonal matrix works.
sig_z=np.array([[sigma_3**2]])
3. Sanity Check Your Kalman Filter
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
A = np.array([[0,1], [0,-d/m]])
B = np.array([[0], [1/m]])
C = np.array([[-1,0]])
sig_u = np.array([[sigma_1**2,0], [0,sigma_2**2]])
sig_z = np.array([[sigma_3**2]])
mu = np.array([[2532], [0]])
def kf(mu,sigma,u,y):
Ad = np.eye(2) + dt * A
Bd = dt * B
mu_p = Ad.dot(mu) + Bd.dot(u)
sig_p = Ad.dot(sigma.dot(Ad.transpose())) + Sigma_u
sigma_m = C.dot(sig_p.dot(C.transpose())) + Sigma_z
kkf_gain = sig_p.dot(C.transpose().dot(np.linalg.inv(sigma_m)))
y_m = y-C.dot(mu_p)
mu = mu_p + kkf_gain.dot(y_m)
sigma = (np.eye(2)-kkf_gain.dot(C)).dot(sig_p)
return mu,sigma
4. Implement the Kalman Filter on the Robot
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
#include <BasicLinearAlgebra.h>
using namespace BLA;
int sigma_3 = 20;
float d = 90.0/1700;
float m = 0.000282291;
Matrix<2,2> A = { 0, 1,
0, -d/m};
Matrix<2,1> B = { 0,
1/m };
Matrix<1,2> C = { -1, 0 };
Matrix<2,2> sig_u = {sigma_1*sigma_1, 0,
0, sigma_2*sigma_2};
Matrix<1> sig_z = {sigma_3 * sigma_3};
Matrix<2,1> mu = {0, 0};
previous_time = millis();
get_tof();
mu = {ToF_reading, 0};
sig = {0, 0,
0, 0};
previous_error = mu(0, 0) - setpoint;
while(millis() - previous_time < timelimit)
{
current_time = millis();
d_t = current_time - previous_time;
A_d = {1, 1,
0, 1-d/m*dt};
B_d = {0,
dt/m};
mu_p = A_d * mu + B_d * 90;
sig_p = A_d * sig * ~A_d + sig_u;
sig_m = C * sig_p * ~C + sig_z;
sig_m = {1/sig_m(0,0);};
inv_sig_m = sig_m;
kf_gain = sig_p * ~C * inv_sig_m;
y = {dis};
y_m = y - C * mu_p;
mu = mu_p + KF_gain * y_m;
sig = (identMtx - kf_gain * C) * sig_p;
}
Test using P = 0.5, I = 0, D = 18
.
Without the Kalman Filter
Video Demo
With the Kalman Filter Video Demo
Compare two outcomes, the one with the Kalman filter has the smoothest motion.