458b12956e
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.
142 lines
2.8 KiB
Python
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
|
|
"""
|