Yes, as long as you have readings for (at least) two base stations in each position you can map the pose of a base station from one position to another.
Each CF position is a reference frame and you need to find the transformation of the base station pose from one reference frame to the next one.
You can try this script out, or use is as inspiration.
Code: Select all
import math
import numpy as np
from scipy.spatial.transform import Rotation
from collections import namedtuple
System = namedtuple("System", ['P', 'R', 'bs'])
def gen_rot_x(angle):
co = math.cos(angle)
si = math.sin(angle)
return np.array([
[1, 0, 0],
[0, co, -si],
[0, si, co]])
def gen_rot_z(angle):
co = math.cos(angle)
si = math.sin(angle)
return np.array([
[co, -si, 0],
[si, co, 0],
[0, 0, 1]])
# Transform a point in ref frame x to ref frame r using transformation x->r
def transform_point_x_to_r(v_x, Px_r, Rx_r):
return np.dot(Rx_r, v_x) + Px_r
# Transform a rotation in ref frame x to ref frame r using transformation x->r
def transform_rot_x_to_r(Rx_y, Ry_r):
return np.dot(Ry_r, Rx_y)
# Transform a base station (y) in ref frame x to ref frame r using transformation x->r
def transform_x_to_r(Sy_x, Sx_r):
Py_r = transform_point_x_to_r(Sy_x.P, Sx_r.P, Sx_r.R)
Ry_r = transform_rot_x_to_r(Sy_x.R, Sx_r.R)
return System(Py_r, Ry_r, Sy_x.bs)
# Transform a point in ref frame r to ref frame x using transformation x->r
def transform_point_to_x_from_r(v_r, Px_r, Rx_r):
Rr_x = np.matrix.transpose(Rx_r)
return np.dot(Rr_x, v_r - Px_r)
# Find the transformation x->r when we know the bs pos/rot for both x and r
def transform_from_ref_x_to_r_same_bs(Sb_x, Sb_r):
Rb_x_inv = np.matrix.transpose(Sb_x.R)
Rx_r = np.dot(Sb_r.R, Rb_x_inv)
Px_r = Sb_r.P - np.dot(Rx_r, Sb_x.P)
return System(Px_r, Rx_r, Sb_x.bs)
# Averaging of quaternions
# From https://stackoverflow.com/a/61013769
def q_average(Q, W=None):
if W is not None:
Q *= W[:, None]
eigvals, eigvecs = np.linalg.eig(Q.T@Q)
return eigvecs[:, eigvals.argmax()]
def system_average(S):
bs = S[0].bs
for s in S:
if s.bs != bs:
raise Exception("Different base stations")
Q = map(lambda s : Rotation.from_matrix(s.R).as_quat(), S)
q = q_average(np.array(list(Q)))
r = Rotation.from_quat(q).as_matrix()
P = map(lambda s : s.P, S)
p = np.average(np.array(list(P)), axis=0)
return System(p, r, bs)
measurements = [
[
# The global ref frame - the origin
System(np.array([-1.4852667 ,-0.10013239, 2.0018834 ]), np.array([[ 0.71420725,-0.44233498, 0.5424461 ], [0.3414208 ,0.89670753,0.28168679], [-0.61101542,-0.01598036, 0.79145738], ]), 2),
System(np.array([ 2.62930353,-0.22387827, 2.2876007]), np.array([[-0.44243481,-0.66061902,-0.60649316], [ 0.51203157,-0.74130071, 0.43393194], [-0.7362575 ,-0.11855705, 0.66623504], ]), 1),
],
[
System(np.array([ 2.63076761,-0.22235061, 2.27780455]), np.array([[-0.44400779,-0.66127997,-0.60462044], [ 0.51142969,-0.74110235, 0.43497929], [-0.73572872,-0.11608665, 0.66725343], ]), 1),
System(np.array([-1.4770189 ,-0.09919998, 2.00858532]), np.array([[ 0.71191171,-0.44151409, 0.54611997], [0.34088548,0.89714765,0.2809327 ], [-0.61398599,-0.01383492, 0.78919566], ]), 2),
],
[
System(np.array([1.8918052 ,0.37315688,2.2956454 ]), np.array([[-0.55202334,-0.43387006,-0.71205828], [ 0.36222251,-0.89395567, 0.26389036], [-0.75104266,-0.11224989, 0.6506419 ], ]), 1),
System(np.array([-2.06908923,-0.60137163, 2.00787968]), np.array([[ 0.60031459,-0.6606275 , 0.45077011], [0.50542771,0.75018765,0.42633475], [-0.61981063,-0.02810327, 0.78424804], ]), 2),
],
[
System(np.array([0.81226372,0.72658043,2.30703026]), np.array([[-0.63100663, 0.18127552,-0.75430088], [-0.02502271,-0.97656606,-0.21375824], [-0.77537378,-0.11600822, 0.62075566], ]), 1),
System(np.array([-1.72116567,-2.38984028, 1.9674524 ]), np.array([[ 0.18445731,-0.97738034, 0.10345616], [0.76810961,0.20902735,0.60523978], [-0.61317463,-0.03217523, 0.78929185], ]), 2),
],
[
System(np.array([0.88302677,0.21773784,1.29904 ]), np.array([[-0.82446229, 0.23252554,-0.51593973], [-0.23580717,-0.96992637,-0.06031429], [-0.51444816, 0.07193542, 0.85449891], ]), 0),
System(np.array([-0.34440593,-1.80757135, 2.14094036]), np.array([[-0.11239338,-0.94578565,-0.30472484], [ 0.69138764,-0.29469574, 0.65964957], [-0.71368821,-0.13654274, 0.68702636], ]), 1),
],
[
System(np.array([-0.08719829, 1.03855911, 1.26059635]), np.array([[-0.21572911, 0.94116655,-0.26012782], [-0.83978171,-0.31475733,-0.44237372], [-0.49822448, 0.1230177 , 0.85827677], ]), 0),
System(np.array([ 0.95282395,-1.10080986, 2.20056662]), np.array([[-0.61788579,-0.2604643 ,-0.74187296], [ 0.27185388,-0.9561156 , 0.10926308], [-0.73777545,-0.13416894, 0.66157848], ]), 1),
],
[
System(np.array([-2.08526947,-0.42735793, 1.1706371 ]), np.array([[ 0.566362 ,-0.74323252, 0.35614536], [0.69140488,0.66366893,0.28548703], [-0.44854585, 0.08455164, 0.88975145], ]), 0),
System(np.array([1.82680506,2.06461834,2.15551761]), np.array([[-0.6515338 , 0.50143071,-0.5692723 ], [-0.37619232,-0.86519582,-0.3315351 ], [-0.6587739 ,-0.00185046, 0.75233871], ]), 2),
],
[
System(np.array([-1.12594278,-2.05115544, 1.18245119]), np.array([[-0.15492359,-0.98792286,-0.00266465], [ 0.87068111,-0.13781152, 0.47214658], [-0.46681162, 0.07082658, 0.88151603], ]), 0),
System(np.array([-0.5475143 , 2.51547338, 2.07912607]), np.array([[-0.11243505, 0.98492681,-0.13144404], [-0.74911736,-0.17092836,-0.64000522], [-0.65282582, 0.02650799, 0.75704411], ]), 2),
],
]
##################
def print_system(system):
print(f"Base station {system.bs} @ {system.P}")
def probe_position(Sbs_ref, Sbs0_other, Sbs1_other):
# Find transform from other ref frame to global using bs0
Sother_g = transform_from_ref_x_to_r_same_bs(Sbs0_other, Sbs_ref)
# The meassurement of base station 1 in the other ref frame, converted to global
Sbs1mOther_g = transform_x_to_r(Sbs1_other, Sother_g)
return Sbs1mOther_g
print()
ref = measurements[0][0]
result = [ref]
found = [ref.bs]
not_done = True
while not_done:
not_done = False
samples = {}
print("--- iteration")
for measurement in measurements:
# Find a reference system in this measurement
from_bs = None
from_sys = None
from_sys_g = None
for system in measurement:
bs = system.bs
for sys in result:
if sys.bs == bs:
from_bs = bs
from_sys = system
from_sys_g = sys
break
if from_bs is not None:
break
# Transform all base stations in this measurement to
# the global system, unless we already have a result for a
# particular base station
if from_bs is not None:
print(f"Using {from_bs} as reference")
for sys in measurement:
if sys is not from_sys:
if sys.bs not in found:
s = probe_position(from_sys_g, from_sys, sys)
from_to = (from_bs, sys.bs)
if not from_to in samples:
samples[from_to] = []
samples[from_to].append(s)
else:
not_done = True
# Average over all samples sets
for from_to, sample_set in samples.items():
new_sys = system_average(sample_set)
result.append(new_sys)
found.append(new_sys.bs)
for sys in result:
print_system(sys)
The layout of sensors on the deck (symmetrical and in one plane) will give us multiple possible (mirrored) solutions to the base station pose. To find the right one we feed the estimator with an initial guess, but that guess is based on the assumption that the crazyflie is flat on the ground and that the base stations are located above the ground (z > 0). It is possible that you find a mirror solution.
In your case (the picture) I think i looks like you should get something like (-1, 0, 2). You can find the definition of the coordinate system here