In [14]:
class KF(object): #old from drop
def Configure(self, zMeasVariance, zAccelVariance, zAccelBiasVariance, zInitial, vInitial, aBiasInitial, paa):
self.zMeasVariance_ = zMeasVariance;
self.zAccelVariance_ = zAccelVariance;
self.zAccelBiasVariance_ = zAccelBiasVariance;
self.z_ = zInitial;
self.v_ = vInitial;
self.aBias_ = aBiasInitial;
self.Pzz_ = 1.0;
self.Pzv_ = 0.0;
self.Pza_ = 0.0;
self.Pvz_ = 0.0;
self.Pvv_ = 1.0;
self.Pva_ = 0.0;
self.Paz_ = 0.0;
self.Pav_ = 0.0;
self.Paa_ = paa
def Update(self, z, a, dt):
accel = a - self.aBias_;
self.v_ += accel * dt;
self.z_ += self.v_ * dt;
#when zAccelVariance is large, filter favours fresh data.
#when small, filter favours existing state.
#Predict State Covariance matrix
dt2div2 = dt*dt/2.0;
dt3div2 = dt2div2*dt;
dt4div4 = dt2div2*dt2div2;
t00 = self.Pzz_ + dt*self.Pvz_ - dt2div2*self.Paz_;
t01 = self.Pzv_ + dt*self.Pvv_ - dt2div2*self.Pav_;
t02 = self.Pza_ + dt*self.Pva_ - dt2div2*self.Paa_;
t10 = self.Pvz_ - dt*self.Paz_;
t11 = self.Pvv_ - dt*self.Pav_;
t12 = self.Pva_ - dt*self.Paa_;
t20 = self.Paz_;
t21 = self.Pav_;
t22 = self.Paa_;
self.Pzz_ = t00 + dt*t01 - dt2div2*t02;
self.Pzv_ = t01 - dt*t02;
self.Pza_ = t02;
self.Pvz_ = t10 + dt*t11 - dt2div2*t12;
self.Pvv_ = t11 - dt*t12;
self.Pva_ = t12;
self.Paz_ = t20 + dt*t21 - dt2div2*t22;
self.Pav_ = t21 - dt*t22;
self.Paa_ = t22;
self.Pzz_ += dt4div4*self.zAccelVariance_;
self.Pzv_ += dt3div2*self.zAccelVariance_;
self.Pvz_ += dt3div2*self.zAccelVariance_;
self.Pvv_ += dt*dt*self.zAccelVariance_;
self.Paa_ += self.zAccelBiasVariance_;
#Error
innov = z - self.z_;
sInv = 1.0 / (self.Pzz_ + self.zMeasVariance_);
# Kalman gains
kz = self.Pzz_ * sInv;
kv = self.Pvz_ * sInv;
ka = self.Paz_ * sInv;
#Update state
self.z_ += kz * innov;
self.v_ += kv * innov;
self.aBias_ += ka * innov;
#Update state covariance matrix
self.Pzz_ -= kz * self.Pzz_;
self.Pzv_ -= kz * self.Pzv_;
self.Pza_ -= kz * self.Pza_;
self.Pvz_ -= kv * self.Pzz_;
self.Pvv_ -= kv * self.Pzv_;
self.Pva_ -= kv * self.Pza_;
self.Paz_ -= ka * self.Pzz_;
self.Pav_ -= ka * self.Pzv_;
self.Paa_ -= ka * self.Pza_;
return self.z_, self.v_
In [8]:
class KF1(object): #old from bean2
def Configure(self):
self.var_accel = 1.0
self.x_abs = 0;
self.x_vel = 0;
self.p_abs_abs = 1.0e10;
self.p_abs_vel = 0.0;
self.p_vel_vel = self.var_accel;
def Update(self, z_abs):
var_z_abs = 0.2
dt = 0.01
# // Note: math is not optimized by hand. Let the compiler sort it out.
# // Predict step.
# // Update state estimate.
self.x_abs += self.x_vel * dt;
# // Update state covariance. The last term mixes in acceleration noise.
self.p_abs_abs += 2.0 * dt * self.p_abs_vel + dt * dt * self.p_vel_vel + self.var_accel * dt * dt * dt * dt / 4.0;
self.p_abs_vel += dt * self.p_vel_vel + self.var_accel * dt * dt * dt / 2.0;
self.p_vel_vel += self.var_accel * dt * dt;
# // Update step.
self.y = z_abs - self.x_abs;
self.s_inv = 1. / (self.p_abs_abs + var_z_abs); #// Innovation precision.
self.k_abs = self.p_abs_abs * self.s_inv; #// Kalman gain
self.k_vel = self.p_abs_vel * self.s_inv;
#// Update state estimate.
self.x_abs += self.k_abs * self.y;
self.x_vel += self.k_vel * self.y;
#// Update state covariance.
self.p_vel_vel -= self.p_abs_vel * self.k_vel;
self.p_abs_vel -= self.p_abs_vel * self.k_abs;
self.p_abs_abs -= self.p_abs_abs * self.k_abs;
return self.x_abs, self.x_vel
In [122]:
class KF2(object): #new
def Configure(self, Q_accel, R_altitude, h, v):
self.Q_accel = Q_accel
self.R_altitude = R_altitude
self.h = h
self.v = v
self.P = [
[1.0, 0.0],
[0.0, 1.0]
]
def propagate(self, acceleration, dt):
_dtdt = dt * dt;
self.h = self.h + self.v * dt + 0.5 * acceleration * _dtdt;
self.v = self.v + acceleration * dt;
_Q_accel_dtdt = self.Q_accel * _dtdt;
self.P[0][0] = self.P[0][0] + (self.P[1][0] + self.P[0][1] + (self.P[1][1] + 0.25*_Q_accel_dtdt) * dt) * dt;
self.P[0][1] = self.P[0][1] + (self.P[1][1] + 0.5*_Q_accel_dtdt) * dt;
self.P[1][0] = self.P[1][0] + (self.P[1][1] + 0.5*_Q_accel_dtdt) * dt;
self.P[1][1] = self.P[1][1] + _Q_accel_dtdt;
def update(self, altitude):
y = altitude - self.h
Sinv = 1.0 / (self.P[0][0] + self.R_altitude)
K = [self.P[0][0] * Sinv, self.P[1][0] * Sinv]
self.h += K[0] * y;
self.v += K[1] * y;
self.P[0][0] = self.P[0][0] - K[0] * self.P[0][0];
self.P[0][1] = self.P[0][1] - K[0] * self.P[0][1];
self.P[1][0] = self.P[1][0] - (K[1] * self.P[0][0]);
self.P[1][1] = self.P[1][1] - (K[1] * self.P[0][1]);
def Update(self, z_abs, accel, dt = 0.01):
self.propagate(accel, dt)
self.update(z_abs)
return self.h, self.v
def Update1(self, z_abs, accel, dt = 0.01):
self.update(z_abs)
self.propagate(accel, dt)
return self.h, self.v
In [123]:
%matplotlib gtk3
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider
import numpy as np
raw_alts = []
raw_accs = []
filt_alts = []
filt_varios = []
#load data
lines = open("data_acc9.csv", "r").readlines()
for line in lines:
raw_alt, raw_acc, filt_alt, filt_vario = map(float, line.split(","))
raw_alts.append(raw_alt)
raw_accs.append(raw_acc * 9.81)
filt_alts.append(filt_alt)
filt_varios.append(filt_vario)
#draw
x = np.arange(len(raw_alts) / 100, step = 0.01)
fig, ax = plt.subplots(2, 1, sharex=True)
ax1, ax2 = ax
plt.subplots_adjust(left=0.03, bottom=0.15, top = 0.98, right = 0.99, hspace = 0)
zero = [None] * len(x)
ax1.plot(x, raw_alts, "o", label="raw data", markersize=0.5)
ax1.plot(x, filt_alts, "c", label = "KF orig")
p_raw_avg, = ax1.plot(x, filt_alts, "y-", label = "raw data average")
p_kf1, = ax1.plot(x, zero, "b-", label = "KF1")
p_kf2, = ax1.plot(x, zero, "g-", label = "KF2")
p_kf3, = ax1.plot(x, zero, "k-", label = "KF3")
ax1.grid(True)
ax2.grid(True)
ax2.hlines(0, 0, max(x))
ax2.plot(x, filt_varios, "c", label = "KF orig")
p_kf1_v, = ax2.plot(x, filt_varios, "b", label = "KF1")
p_kf2_v, = ax2.plot(x, zero, "g", label = "KF2")
p_kf3_v, = ax2.plot(x, zero, "k", label = "KF3")
p_z_acc, = ax2.plot(x, raw_accs, "m", label = "Z acc")
p_z_acc_int, = ax2.plot(x, raw_accs, "r", label = "Z int")
ax1.legend(loc='best', shadow=True)
ax2.legend(loc='best', shadow=True)
def redraw(val1, val2, val3, val4, val5):
print(val1, val2, val3, val4, val5)
avg_alts = []
avg_alt = None
nkf1_alts = [None]
nkf1_varios = [None]
nkf2_alts = [None]
nkf2_varios = [None]
nkf3_alts = [None]
nkf3_varios = [None]
faccs = [None]
diffs = []
dlen = 50
gain = 2
filt = 50
facc = 0
acc_int = 0
acc_ints = [None]
i = 0
for line in lines:
raw_alt, raw_acc, filt_alt, filt_vario = map(float, line.split(","))
raw_acc *= 9.81
if not avg_alt:
avg_alt = raw_alt
# kf = KF()
# kf.Configure(zVar, zAccVar, 1.0, raw_alt, 0.0, 0.0, paa)
kf1 = KF1()
kf1.Configure()
kf2 = KF1()
kf2.Configure()
kf3 = KF2()
kf3.Configure(val3, val4, raw_alt, 0)
else:
if val2 == 0:
raw_acc = 0;
if val2 < 1:
facc = raw_acc
else:
facc += (raw_acc - facc) / val2
faccs.append(facc)
acc_int += facc
acc_ints.append(acc_int * (val1 / 100.0) )
avg_alt += (raw_alt - avg_alt) / filt
nkf1_alt, nkf1_vario = kf1.Update(raw_alt)
nkf1_alts.append(nkf1_alt)
nkf1_varios.append(nkf1_vario)
nkf2_alt, nkf2_vario = kf2.Update(raw_alt)
nkf2_vario += acc_int * (val1 / 100.0)
acc_int *= 0.99
nkf2_alts.append(nkf2_alt)
nkf2_varios.append(nkf2_vario)
if val1 == 0:
nkf3_alt, nkf3_vario = kf3.Update(raw_alt, raw_acc * val5, 0.01)
else:
nkf3_alt, nkf3_vario = kf3.Update1(raw_alt, raw_acc * val5, 0.01)
nkf3_alts.append(nkf3_alt)
nkf3_varios.append(nkf3_vario)
if len(avg_alts) >= dlen and avg_alts[-dlen]:
val = avg_alt - avg_alts[-dlen]
val *= gain
diffs.append(val)
else:
diffs.append(None)
avg_alts.append(avg_alt)
i += 1
avg_alts = avg_alts[int(filt/2):] + [None] * int(filt/2)
p_raw_avg.set_ydata(avg_alts)
p_kf1.set_ydata(nkf1_alts)
p_kf1_v.set_ydata(nkf1_varios)
p_kf2.set_ydata(nkf2_alts)
p_kf2_v.set_ydata(nkf2_varios)
p_kf3.set_ydata(nkf3_alts)
p_kf3_v.set_ydata(nkf3_varios)
p_z_acc.set_ydata(faccs)
p_z_acc_int.set_ydata(acc_ints)
fig.canvas.draw_idle()
def update(val = None):
redraw(val1.val, val2.val, val3.val, val4.val, val5.val)
axvar1 = plt.axes([0.2, 0.09, 0.65, 0.02])
axvar2 = plt.axes([0.2, 0.07, 0.65, 0.02])
axvar3 = plt.axes([0.2, 0.05, 0.65, 0.02])
axvar4 = plt.axes([0.2, 0.03, 0.65, 0.02])
axvar5 = plt.axes([0.2, 0.01, 0.65, 0.02])
val1 = Slider(axvar1, 'KF2 A int gain [%]', 0, 10, valinit=2)
val2 = Slider(axvar2, 'ACC floating avg [samples]', 0, 100, valinit=10)
val3 = Slider(axvar3, 'KF3 ACC covar', 0, 60, valinit=30)
val4 = Slider(axvar4, 'KF3 H covar', 0, 20, valinit=4)
val5 = Slider(axvar5, 'KF3 Acc gain', 0, 2, valinit=1)
val1.on_changed(update)
val2.on_changed(update)
val3.on_changed(update)
val4.on_changed(update)
val5.on_changed(update)
plt.show()
update()
In [ ]:
In [ ]: