Kalman-and-Bayesian-Filters.../experiments/ILS.py
Roger Labbe 458b12956e Fixed diverging UKF filter.
The UKF for the circle tracking was diverging. Also,
I have partially started the section on the ILS, but
much remains to be done on it.
2015-02-27 16:23:53 -08:00

142 lines
2.8 KiB
Python

# -*- coding: utf-8 -*-
import numpy as np
from numpy.linalg import norm, inv
from numpy.random import randn
from numpy import dot
numpy.random.seed(1234)
user_pos = np.array([1000, 100]) # d5, D6
pred_user_pos = np.array([100, 0]) #d7, d8
t_pos = np.asarray([[0, 1000],
[0, -1000],
[500, 500]], dtype=float)
def transmitter_range(pos, transmitter_pos):
""" Compute distance between position 'pos' and the list of positions
in transmitter_pos"""
N = len(transmitter_pos)
rng = np.zeros(N)
diff = np.asarray(pos) - transmitter_pos
for i in range(N):
rng[i] = norm(diff[i])
return norm(diff, axis=1)
# compute measurement of where you are with respect to seach sensor
rz= transmitter_range(user_pos, t_pos) # $B21,22
# add some noise
for i in range(len(rz)):
rz[i] += randn()
# now iterate on the predicted position
pos = pred_user_pos
def hx_range(pos, t_pos, r_est):
N = len(t_pos)
H = np.zeros((N, 2))
for j in range(N):
H[j,0] = -(t_pos[j,0] - pos[0]) / r_est[j]
H[j,1] = -(t_pos[j,1] - pos[1]) / r_est[j]
return H
def lop_ils(zs, t_pos, pos_est, hx, eps=1.e-6):
""" iteratively estimates the solution to a set of measurement, given
known transmitter locations"""
pos = np.array(pos_est)
converged = False
for i in range(20):
r_est = transmitter_range(pos, t_pos) #B32-B33
print('iteration:', i)
#print ('ra1, ra2', ra1, ra2)
print()
H=hx(pos, t_pos, r_est)
Hinv = inv(dot(H.T, H)).dot(H.T)
#update position estimate
y = zs - r_est
print('residual', y)
Hy = np.dot(Hinv, y)
print('Hy', Hy)
pos = pos + Hy
print('pos', pos)
print()
print()
if max(abs(Hy)) < eps:
converged = True
break
return pos, converged
print(lop_ils(rz, t_pos, (900,90), hx=hx_range))
#####################
"""
# compute measurement (simulation)
rza1, rza2 = transmitter_range(user_pos) # $B21,22
rza1 += randn()
rza2 += randn()
# now iterate on the predicted position
pos = pred_user_pos
for i in range(10):
ra1, ra2 = transmitter_range(pos) #B32-B33
print('iteration:', i)
print ('ra1, ra2', ra1, ra2)
print()
H = np.array([[-(t1_pos[0] - pos[0]) / ra1, -(t1_pos[1] - pos[1]) / ra1],
[-(t2_pos[0] - pos[0]) / ra2, -(t2_pos[1] - pos[1]) / ra2]])
Hinv = inv(H)
#update position estimate
residual_t1 = rza1 - ra1
residual_t2 = rza2 - ra2
y = np.array([[residual_t1], [residual_t2]])
print('residual', y.T)
Hy = np.dot(Hinv, y)
pos = pos + Hy[:,0]
print('pos', pos)
print()
print()
if (max(abs(y)) < 1.e-6):
break
"""