def show_sigmas_for_2_kappas():
# generate the Gaussian data
xs = np.arange(-4, 4, 0.1)
mean = 0
sigma = 1.5
ys = [stats.gaussian(x, mean, sigma*sigma) for x in xs]
#generate our samples
kappa = 2
x0,x1,x2 = _sigma_points(mean, sigma, kappa)
samples = [x0,x1,x2]
for x in samples:
p1 = plt.scatter([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='k')
kappa = -.5
x0,x1,x2 = _sigma_points(mean, sigma, kappa)
samples = [x0,x1,x2]
for x in samples:
p2 = plt.scatter([x], [stats.gaussian(x, mean, sigma*sigma)], s=80, color='b')
plt.legend([p1,p2], ['$kappa$=2', '$kappa$=-0.5'])
plt.plot(xs, ys)
plt.show()
def show_sigmas_for_2_kappas():
# generate the Gaussian data
xs = np.arange(-4, 4, 0.1)
mean = 0
sigma = 1.5
ys = [stats.gaussian(x, mean, sigma * sigma) for x in xs]
#generate our samples
kappa = 2
x0, x1, x2 = _sigma_points(mean, sigma, kappa)
samples = [x0, x1, x2]
for x in samples:
p1 = plt.scatter([x], [stats.gaussian(x, mean, sigma * sigma)],
s=80,
color='k')
kappa = -.5
x0, x1, x2 = _sigma_points(mean, sigma, kappa)
samples = [x0, x1, x2]
for x in samples:
p2 = plt.scatter([x], [stats.gaussian(x, mean, sigma * sigma)],
s=80,
color='b')
plt.legend([p1, p2], ['$kappa$=2', '$kappa$=-0.5'])
plt.plot(xs, ys)
plt.show()
def gaussian_vs_histogram():
seed(15)
xs = np.arange(0, 20, 0.1)
ys = np.array([stats.gaussian(x-10, 0, 2) for x in xs])
bar_ys = abs(ys + randn(len(xs)) * stats.gaussian(xs-10, 0, 10)/2)
plt.gca().bar(xs[::5]-.25, bar_ys[::5], width=0.5, color='g')
plt.plot(xs, ys, lw=3, color='k')
plt.xlim(5, 15)
def gaussian_vs_histogram():
seed(15)
xs = np.arange(0, 20, 0.1)
ys = np.array([stats.gaussian(x - 10, 0, 2) for x in xs])
bar_ys = abs(ys + randn(len(xs)) * stats.gaussian(xs - 10, 0, 10) / 2)
plt.gca().bar(xs[::5] - .25, bar_ys[::5], width=0.5, color='g')
plt.plot(xs, ys, lw=3, color='k')
plt.xlim(5, 15)
def show_3_sigma_points():
xs = np.arange(-4, 4, 0.1)
var = 1.5
ys = [stats.gaussian(x, 0, var) for x in xs]
samples = [0, 1.2, -1.2]
for x in samples:
plt.scatter([x], [stats.gaussian(x, 0, var)], s=80)
plt.plot(xs, ys)
plt.show()
def show_3_sigma_points():
xs = np.arange(-4, 4, 0.1)
var = 1.5
ys = [stats.gaussian(x, 0, var) for x in xs]
samples = [0, 1.2, -1.2]
for x in samples:
plt.scatter ([x], [stats.gaussian(x, 0, var)], s=80)
plt.plot(xs, ys)
plt.show()
def gausshist(self):
seed(15)
xs = np.arange(0, 20, 0.1)
ys = np.array([stats.gaussian(x - 10, 0, 2) for x in xs])
np.set_printoptions(formatter={'float': '{: 0.2f}'.format})
print(xs)
print(ys)
bar_ys = abs(ys + randn(len(xs)) * stats.gaussian(xs - 10, 0, 10) / 2)
plt.gca().bar(xs[::5] - .25, bar_ys[::5], width=0.5, color='g')
plt.plot(xs, ys, lw=3, color='k')
plt.xlim(-20, +20)
return plt
def plot_products(self, m1, v1, m2, v2, legend=True):
plt.figure()
product = self.gaussian_multiply((m1, v1), (m2, v2))
xs = np.arange(5, 15, 0.1)
ys = [stats.gaussian(x, m1, v1) for x in xs]
plt.plot(xs, ys, label='$\mathcal{N}$' + '$({},{})$'.format(m1, v1))
ys = [stats.gaussian(x, m2, v2) for x in xs]
plt.plot(xs, ys, label='$\mathcal{N}$' + '$({},{})$'.format(m2, v2))
ys = [stats.gaussian(x, *product) for x in xs]
plt.plot(xs, ys, label='product', ls='--')
if legend:
plt.legend()
return plt
def plot_gaussian_multiply():
xs = np.arange(-5, 10, 0.1)
mean1, var1 = 0, 5
mean2, var2 = 5, 1
mean, var = stats.mul(mean1, var1, mean2, var2)
ys = [stats.gaussian(x, mean1, var1) for x in xs]
plt.plot(xs, ys, label='M1')
ys = [stats.gaussian(x, mean2, var2) for x in xs]
plt.plot(xs, ys, label='M2')
ys = [stats.gaussian(x, mean, var) for x in xs]
plt.plot(xs, ys, label='M1 x M2')
plt.legend()
plt.show()
def plot_gaussian_multiply():
xs = np.arange(-5, 10, 0.1)
mean1, var1 = 0, 5
mean2, var2 = 5, 1
mean, var = stats.mul(mean1, var1, mean2, var2)
ys = [stats.gaussian(x, mean1, var1) for x in xs]
plt.plot(xs, ys, label='M1')
ys = [stats.gaussian(x, mean2, var2) for x in xs]
plt.plot(xs, ys, label='M2')
ys = [stats.gaussian(x, mean, var) for x in xs]
plt.plot(xs, ys, label='M1 x M2')
plt.legend()
plt.show()
def display_stddev_plot():
xs = np.arange(10, 30, 0.1)
var = 8
stddev = math.sqrt(var)
p2, = plt.plot(xs, [stats.gaussian(x, 20, var) for x in xs])
x = 20 + stddev
# 1std vertical lines
y = stats.gaussian(x, 20, var)
plt.plot([x, x], [0, y], 'g')
plt.plot([20 - stddev, 20 - stddev], [0, y], 'g')
#2std vertical lines
x = 20 + 2 * stddev
y = stats.gaussian(x, 20, var)
plt.plot([x, x], [0, y], 'g')
plt.plot([20 - 2 * stddev, 20 - 2 * stddev], [0, y], 'g')
y = stats.gaussian(20, 20, var)
plt.plot([20, 20], [0, y], 'b')
x = 20 + stddev
ax = plt.axes()
ax.annotate('68%', xy=(20.3, 0.045))
ax.annotate('',
xy=(20 - stddev, 0.04),
xytext=(x, 0.04),
arrowprops=dict(arrowstyle="<->", ec="r", shrinkA=2,
shrinkB=2))
ax.annotate('95%', xy=(20.3, 0.02))
ax.annotate('',
xy=(20 - 2 * stddev, 0.015),
xytext=(20 + 2 * stddev, 0.015),
arrowprops=dict(arrowstyle="<->", ec="r", shrinkA=2,
shrinkB=2))
ax.xaxis.set_ticks(
[20 - 2 * stddev, 20 - stddev, 20, 20 + stddev, 20 + 2 * stddev])
ax.xaxis.set_ticklabels(
['$-2\sigma$', '$-1\sigma$', '$\mu$', '$1\sigma$', '$2\sigma$'])
ax.yaxis.set_ticks([])
ax.grid(None, 'both', lw=0)
plt.show()
def display_stddev_plot():
xs = np.arange(10,30,0.1)
var = 8; stddev = math.sqrt(var)
p2, = plt.plot (xs,[stats.gaussian(x, 20, var) for x in xs])
x = 20+stddev
y = stats.gaussian(x, 20, var)
plt.plot ([x,x], [0,y],'g')
plt.plot ([20-stddev, 20-stddev], [0,y], 'g')
y = stats.gaussian(20,20,var)
plt.plot ([20,20],[0,y],'b')
ax = plt.axes()
ax.annotate('68%', xy=(20.3, 0.045))
ax.annotate('', xy=(20-stddev,0.04), xytext=(x,0.04),
arrowprops=dict(arrowstyle="<->",
ec="r",
shrinkA=2, shrinkB=2))
ax.xaxis.set_ticks ([20-stddev, 20, 20+stddev])
ax.xaxis.set_ticklabels(['$-\sigma$','$\mu$','$\sigma$'])
ax.yaxis.set_ticks([])
plt.show()
def display_stddev_plot():
xs = np.arange(10,30,0.1)
var = 8;
stddev = math.sqrt(var)
p2, = plt.plot (xs,[stats.gaussian(x, 20, var) for x in xs])
x = 20+stddev
# 1std vertical lines
y = stats.gaussian(x, 20, var)
plt.plot ([x,x], [0,y],'g')
plt.plot ([20-stddev, 20-stddev], [0,y], 'g')
#2std vertical lines
x = 20+2*stddev
y = stats.gaussian(x, 20, var)
plt.plot ([x,x], [0,y],'g')
plt.plot ([20-2*stddev, 20-2*stddev], [0,y], 'g')
y = stats.gaussian(20,20,var)
plt.plot ([20,20],[0,y],'b')
x = 20+stddev
ax = plt.gca()
ax.annotate('68%', xy=(20.3, 0.045))
ax.annotate('', xy=(20-stddev,0.04), xytext=(x,0.04),
arrowprops=dict(arrowstyle="<->",
ec="r",
shrinkA=2, shrinkB=2))
ax.annotate('95%', xy=(20.3, 0.02))
ax.annotate('', xy=(20-2*stddev,0.015), xytext=(20+2*stddev,0.015),
arrowprops=dict(arrowstyle="<->",
ec="r",
shrinkA=2, shrinkB=2))
ax.xaxis.set_ticks ([20-2*stddev, 20-stddev, 20, 20+stddev, 20+2*stddev])
ax.xaxis.set_ticklabels(['$-2\sigma$', '$-1\sigma$','$\mu$','$1\sigma$', '$2\sigma$'])
ax.yaxis.set_ticks([])
ax.grid(None, 'both', lw=0)
def binsAndGauss(self):
pass
seed(15)
xs07 = np.arange(-2, 2, 0.001)
ys07 = np.array([stats.gaussian(x, 0.7, (0.6 / 3)**2) for x in xs07])
ys04 = np.array([stats.gaussian(x, 0.15, (0.4 / 3)**2) for x in xs07])
ys_m = np.array([stats.gaussian(x, 0.32, (0.1)**2) for x in xs07])
#np.set_printoptions(formatter={'float': '{: 0.2f}'.format})
#print(xs)
#print(ys)
#bar_ys = abs(ys + randn(len(xs)) * stats.gaussian(xs-10, 0, 10)/2)
#plt.gca().bar(xs[::5]-.25, bar_ys[::5], width=0.5, color='g')
plt.bar([0.4], [2], 0.8, color="yellow")
plt.bar([0.7], [4], 0.8, color="gray")
plt.plot(xs07, ys04, lw=3, color='k')
plt.plot(xs07, ys07, lw=3, color='k')
plt.plot(xs07, ys_m, lw=3, color='red')
plt.xlim(-2, +2)
plt.show()
return plt
def plot_gaussian (mu, variance,
mu_line=False,
xlim=None,
xlabel=None,
ylabel=None):
xs = np.arange(mu-variance*2,mu+variance*2,0.1)
ys = [stats.gaussian (x, mu, variance)*100 for x in xs]
plt.plot (xs, ys)
if mu_line:
plt.axvline(mu)
if xlim:
plt.xlim(xlim)
if xlabel:
plt.xlabel(xlabel)
if ylabel:
plt.ylabel(ylabel)
plt.show()
def plot_gaussian (mu, variance,
mu_line=False,
xlim=None,
xlabel=None,
ylabel=None):
xs = np.arange(mu-variance*2,mu+variance*2,0.1)
ys = [stats.gaussian (x, mu, variance)*100 for x in xs]
plt.plot (xs, ys)
if mu_line:
plt.axvline(mu)
if xlim:
plt.xlim(xlim)
if xlabel:
plt.xlabel(xlabel)
if ylabel:
plt.ylabel(ylabel)
plt.show()
from numpy.random import randn
import filterpy.stats as stats
import matplotlib.pyplot as plt
from collections import namedtuple
gaussian = namedtuple('Gaussian', ['mean', 'var'])
gaussian.__repr__ = lambda s: '𝒩(μ={:.3f}, 𝜎²={:.3f})'.format(s[0], s[1])
def gaussian_multiply(g1, g2):
mean = (g1.var * g2.mean + g2.var * g1.mean) / (g1.var + g2.var)
variance = (g1.var * g2.var) / (g1.var + g2.var)
return gaussian(mean, variance)
z = gaussian(10., 1.) # Gaussian N(10, 1)
product = gaussian_multiply(z, z)
xs = np.arange(5, 15, 0.1)
ys = [stats.gaussian(x, z.mean, z.var) for x in xs]
plt.plot(xs, ys, label='$\mathcal{N}(10,1)$')
ys = [stats.gaussian(x, product.mean, product.var) for x in xs]
plt.plot(xs,
ys,
label='$\mathcal{N}(10,1) \\times \mathcal{N}(10,1)$',
ls='--')
plt.legend()
print(product)
from filterpy.stats import norm_cdf
from filterpy.stats import gaussian
import numpy as np
from filterpy.stats import plot_gaussian_pdf
u1, u2 = 10, 32
var1, var2 = 25, 100
xs = u1
G1 = gaussian(xs, u1, var1)
xs = u2
G2 = gaussian(xs, u2, var2)
print(G1, '\n', G2)
xs = np.arange(-100, 100, .1)
sum1, sum2 = 0, 0
sum3 = 0
for i in xs:
sum1 = sum1 + gaussian(i, u1, var1)
sum2 = sum2 + gaussian(i, u2, var2)
sum3 = sum3 + gaussian(i, u1, var1) * gaussian(i, u2, var2)
# Sum 3 is like nothing. Multiplication is not correct, have to change u's and var's myself
print('\n\t', sum1, '\n\t', sum2, '\n\t', sum3)
a = datetime.datetime.now()
current = 0
previous = 0
### sqrt of the variance is the error distance (aka. meters or degrees) ###
# how much error there is in the process model (how much do we trust the model)
process_var = 4.
# how much error there is in each sensor measurement (how much do we trust the sensor)
sensor_var = .3
## Initial State ##
# [position, variance, sensor_var] sensor state
# [(meters, degrees), (meters, degrees), ] ## remember sqrt of variance is distance error
x = gaussian(0., 0., sensor_var)
#[position, velocity, process_var] of model initial state
# how we think the system works
process_model = gaussian(0., 0., process_var)
def calibrate_external_compass():
global ext_magXmax
global ext_magXmin
global ext_magYmax
global ext_magYmin
global ext_magZmin
global ext_magZmax
mag_x, mag_y, mag_z = sensor.magnetic