Это интерактивная методичка по калибровке магнитометра. Методичка не просто расскажет как откалибровать магнитометр, но и откалибрует его вместо Вас. Заодно покажет 3d графики, которые можно вращать и промежуточные этапы вычислений.
from IPython.display import HTML
HTML('''<script>
code_show=true;
function code_toggle() {
if (code_show){
$('div.input').hide();
} else {
$('div.input').show();
}
code_show = !code_show
}
$( document ).ready(code_toggle);
</script>
Исходный код для этого Jupyter notebook для Python по умолчанию скрыт для простоты чтения.
Для отображения/скрытия исходного кода нажмите <a href="javascript:code_toggle()">здесь</a>.''')
Автор методички Зайнулла Жумаев.
Компания СПУТНИКС
from __future__ import print_function
from IPython.display import display, clear_output
from ipywidgets import interact, interactive, fixed, interact_manual
import ipywidgets as widgets
# for data import from files
import pandas as pd
# for open filedialog
from tkinter import filedialog
from tkinter import *
# for plotly graphics (alternative to matplotlib method)
import plotly
from plotly.graph_objs import Scatter, Layout, Data, Surface
import plotly.plotly as py
import plotly.graph_objs as go
import numpy as np
from numpy import *
from numpy import linspace, pi
from scipy.optimize import minimize
# regular expressions
import re
# Функция импортирования файла
root = Tk()
# Hide the main window
root.withdraw()
root.call('wm', 'attributes', '.', '-topmost', True)
clear_output()
infile = filedialog.askopenfilename(multiple=False,filetypes = (("text files","*.txt"),("all files","*.*")))
%gui tk
all_lines = []
with open(infile,"r") as f:
all_lines = f.readlines() # readlines() возвращает список строк файла
for i in range(30,40): # вывод начинаем с 30 строки, т.к. в начале файла может находиться несущественная информация
print(all_lines[i],end="") # вывод без добавления переходов на новую строку
11: x=263 y=55 z=-611 onmessage 12: x=85 y=-244 z=-409 onmessage 13: x=29 y=-254 z=-395 onmessage 14: x=-105 y=-170 z=-443 onmessage 15: x=-329 y=-246 z=-190 onmessage
Т.е. строки, в которых 3 раза встречается символ '=', между которыми не менее одного другого символа.
Выбирать такие строки будем с помощью регулярных выражений и библиотеки re для Python Проверить как работают регулярные выражения можно и в Notepad++ как показано в анимации ниже.
def is_data_line(str_input):
p=re.compile('^.+=.+=.+=.+$')
out = p.match(str_input)!=None
return out
data_lines = [line for line in all_lines if is_data_line(line)]
for i in range(0,5):
print(data_lines[i],end="")
6: x=102 y=779 z=-50 7: x=453 y=645 z=-268 8: x=572 y=570 z=14 9: x=503 y=623 z=56 10: x=450 y=525 z=-445
В качестве разделителей опять будем использовать регулярные выражения вида:
Работу регулярного выражения снова можно проверить в Notepad++
splited_data = [re.split('[0-9]+: x=| y=| z=|\n',line) for line in data_lines]
for i in range(0,5):
print(splited_data[i])
['', '102', '779', '-50 ', ''] ['', '453', '645', '-268 ', ''] ['', '572', '570', '14 ', ''] ['', '503', '623', '56 ', ''] ['', '450', '525', '-445 ', '']
Первый и последний столбцы - пустые, выберем все остальные, т.е. столбцы 1, 2, 3 или 1:4. Первый столбец имеет индекс 0!
str_data = np.array(splited_data)[:,1:4]
for i in range(0,5):
print(str_data[i])
['102' '779' '-50 '] ['453' '645' '-268 '] ['572' '570' '14 '] ['503' '623' '56 '] ['450' '525' '-445 ']
points_raw = np.array([list(map(float, value)) for value in str_data])
print('\nданные преобразованы в тип float:')
for i in range(0,5):
print(points_raw[i])
данные преобразованы в тип float: [ 102. 779. -50.] [ 453. 645. -268.] [ 572. 570. 14.] [ 503. 623. 56.] [ 450. 525. -445.]
# Функции для отображения различных типов графических объектов
# фунция для формирования облака точек заданного цвета в виде графического объекта
def scatter_graph(points, sc_color):
x, y, z = [points[:,i] for i in [0,1,2]]
trace = go.Scatter3d(
x=x,
y=y,
z=z,
mode='markers',
marker=dict(
size=4,
line=dict(
color=sc_color,
width=0.01
),
opacity=0.0
)
)
return trace
###########################################################
# фунция для формирования сферы в виде графического объекта
def sphere_graph(x0,y0,z0,R):
theta = linspace(0,2*pi,100)
phi = linspace(0,pi,100)
x = R*outer(cos(theta),sin(phi))+x0
y = R*outer(sin(theta),sin(phi))+y0
z = R*outer(ones(100),cos(phi))+z0
spheredata = Data([
Surface(
x=x,
y=y,
z=z,
opacity=0.4
)
])
return spheredata[0]
###########################################################
# фунция для формирования эллипсоида в виде графического объекта
def ellipsoid_graph(x0,y0,z0,a,b,c):
theta = linspace(0,2*pi,100)
phi = linspace(0,pi,100)
x = a*outer(cos(theta),sin(phi))+x0
y = b*outer(sin(theta),sin(phi))+y0
z = c*outer(ones(100),cos(phi))+z0
ellipsoiddata = Data([
Surface(
x=x,
y=y,
z=z,
opacity=0.4
)
])
return ellipsoiddata[0]
###########################################################
# функция для отрисовки нескольких графических объектов вместе
def show_graphs(graphs_data):
# data = [trace1, spheredata[0]]
layout = go.Layout(
margin=dict(
l=0,
r=0,
b=0,
t=0
)
)
plotly.offline.init_notebook_mode(connected=True)
fig = go.Figure(data=graphs_data, layout=layout)
plotly.offline.iplot(fig, filename='simple-3d-scatter.html')
График можно вращать, приближать или отдалять.
sc_color='rgb(50, 50, 50)'
show_graphs([scatter_graph(points_raw,sc_color)])
Cкорее всего у вас несколько точек будут "выбиваться из общей картины" и будут расположены далеко от остальных измерений.
Как вы уже увидели выше из графика, иногда датчик может выдавать данные, которые явно являются ошибочными. Ошибочные данные - те которые удалены от поверхности эллипсоида сформированной большинством измерений.
Удалим 2% точек наиболее удаленных от начала координат, т.е. данные с самыми большими по модулю значениями.
Удалим 2% точек внутри и 2% точек снаружи эллипсоида наиболее удаленных от поверхности эллипсоида.
Красным цветом отмечены данные условно принятые шумом. Вместе с данными отрисован эллипсоид, который наилучшим образом описывает множество данных измерений. Вращение не учитывается - главные оси эллипсоида параллельны осям системы координат.
# Первый фильтр - уберем данные наиболее удаленные от начала координат
x, y, z = [points_raw[:,i] for i in [0,1,2]]
zero_distances = x**2+y**2+z**2 # определим расстояние до начала координат для каждой точки
# отсортируем по удаленности от начала координат
sorted_order = np.argsort(zero_distances)
points_temp_1 = points_raw[sorted_order]
length = points_temp_1.shape[0] # определим общее количество точек
start_filt1 = 0
end_filt1 = int(length*0.98) # 2% наиболее удаленных точек от начала координат не рассматриваем
points_filtered_1 = points_temp_1[start_filt1:end_filt1]
noize_data_1 = points_temp_1[end_filt1:]
##############################################################
# Функция для определения параметров эллипсоида (без учета поворота)
def ellipsoid(params):
x0,y0,z0,a,b,c = params
deltas = ((x-x0)**2)/(a**2)+((y-y0)**2)/(b**2)+((z-z0)**2)/(c**2)-1
out = np.sum((np.sum(deltas**2))**2)
return out
params0 = [0,0,0,1,1,1] #соответствует сфере с центром в начале координат единичного радиуса
bnds = ((None,None),(None,None),(None,None),(0,None),(0,None),(0,None)) #нет ограничения на координаты центра эллипсоида; a,b,c>0
##############################################################
# Второй фильтр - из оставшихся точек уберем точки наиболее удаленные от поверхности эллипсоида
x, y, z = [points_filtered_1[:,i] for i in [0,1,2]]
# Определим параметры эллипсоида
ellipsoid_res = minimize(ellipsoid, params0, tol=1e-3, bounds = bnds)
x0,y0,z0,a,b,c = ellipsoid_res.x
# Определим степень удаленности каждой точки от поверхности эллипсоида
deltas = (((x-x0)**2)/(a**2)+((y-y0)**2)/(b**2)+((z-z0)**2)/(c**2)-1)**2
# Отсортируем данные по степени удаленности от поверхности эллипсоида
sorted_order = np.argsort(deltas)
points_temp_2 = points_filtered_1[sorted_order]
length = points_filtered_1.shape[0]
# Отбросим 2% точек сначала и 2% точек с конца списка - наиболее удаленные точки от поверхности эллипсоида
start_filt2 = int(length*0.02)
end_filt2 = int(length*0.98)
points_filtered = points_temp_2[start_filt2:end_filt2]
noize_data_2 = np.append(points_temp_2[:start_filt2],points_temp_2[end_filt2:],axis=0)
noize_data = np.append(noize_data_1,noize_data_2,axis=0)
sc_color_1='rgb(150, 150, 150)'
sc_color_2='rgb(255, 0, 0)'
# show_graphs([scatter_graph(points_filtered,sc_color_1),scatter_graph(noize_data,sc_color_2)])
show_graphs([scatter_graph(points_filtered,sc_color_1),scatter_graph(noize_data,sc_color_2),ellipsoid_graph(x0,y0,z0,a,b,c)])
Радис сферы равен среднему удалению точек от центра распределения. Центр сферы находится в начале координат.
x, y, z = [points_filtered[:,i] for i in [0,1,2]]
#средние значения по x, y, z
xmean = np.mean(x)
ymean = np.mean(y)
zmean = np.mean(z)
#средний радиус сферы
rmean = np.mean(np.sqrt((x-xmean)**2+(y-ymean)**2+(z-zmean)**2))
show_graphs([scatter_graph(points_filtered,sc_color_1),sphere_graph(0,0,0,rmean)])
Полученные данные хорошо аппроксимируются эллипсоидом, хотя на самом деле должны находится на сфере с центром в начале координат, т.к. измерения проводились в постоянном магнитном поле Земли.
Притянуть данные к сфере из эллипсоида можно путем добавления вектора смещения, который сдвинет центр эллипсоида в начало координат, и умножения на матрицу, которая развернет главные оси эллипсоида параллельно осям системы координат, смасштабирует эллипсоид по каждой оси, чтобы он стал сферой.
Соответственно матрица преобразования представляет собой произведение 4-х матриц:
$\begin{bmatrix} A & 0 & 0\\ 0& B & 0 \\ 0 & 0 & C \end{bmatrix} \cdot \begin{bmatrix} 1& 0 & 0\\ 0& \cos\alpha &-\sin\alpha \\ 0& \sin\alpha &\cos\alpha \end{bmatrix} \cdot \begin{bmatrix} \cos\beta & 0 & \sin\beta\\ 0& 1 & 0 \\ -\sin\beta & 0 &\cos\beta \end{bmatrix} \cdot \begin{bmatrix} \cos\gamma& \sin\gamma & 0\\ -\sin\gamma& \cos\gamma & 0 \\ 0& 0 & 1 \end{bmatrix} \cdot \left( \begin{bmatrix} mx_1 & ... & mx_i & ... & mx_n\\ my_1 & ... & my_i & ... & my_n\\ mz_1 & ... & mz_i & ... & mz_n \end{bmatrix}
\right)$
Для произвольной матрицы преобразования и вектора смещения вычисляется функционал "похожести" полученных после преобразования данных со сферой. Путем минимизации этого функционала находится такая матрица и вектор смещения при которых откалиброванные данные наиболее близки к сфере.
def f_mag_cal(params):
A_scale = np.diag(params[0:3])
ax, ay, az = params[3:6]
Rotx = np.array([[1,0,0],[0,cos(ax),-sin(ax)],[0,sin(ax),cos(ax)]])
Roty = np.array([[cos(ay),0,sin(ay)],[0,1,0],[-sin(ay),0,cos(ay)]])
Rotz = np.array([[cos(az),-sin(az),0],[sin(az),cos(az),0],[0,0,1]])
A_cal = np.dot(A_scale,np.dot(Rotx,np.dot(Roty,Rotz)))
b = np.array(params[6:]).reshape(3,1)
# Attention! Probably there are x, y, z definition in several plases
x, y, z = [points_filtered[:,i] for i in [0,1,2]]
xmean = np.mean(x)
ymean = np.mean(y)
zmean = np.mean(z)
rmean = np.mean(np.sqrt((x-xmean)**2+(y-ymean)**2+(z-zmean)**2))
# df_cal = np.dot(A_cal,points_filtered.transpose()).transpose()+b.transpose()
df_cal = np.dot(A_cal,(points_filtered+b.transpose()).transpose()).transpose()
df_cal_uni = df_cal/rmean
out = np.sum((np.sum(df_cal_uni**2,axis=1)-1)**2)
return out
max_angle = 15/180*pi
bnds = ((None, None),(None, None),(None, None),
(-max_angle,max_angle),(-max_angle,max_angle),(-max_angle,max_angle),
(None, None),(None, None),(None, None))
params0 = [1,1,1,0,0,0,0,0,0] #начальное приближение - масштабирование 1 по каждой оси, поворот 0 по каждой оси, сдиг 0 по каждой оси
res = minimize(f_mag_cal, params0, tol=1e-3, bounds = bnds, method = 'SLSQP')
clear_output()
params = np.array(res.x)
# print('params=',params)
A_scale = np.diag(params[0:3])
ax, ay, az = params[3:6]
Rotx = np.array([[1,0,0],[0,cos(ax),-sin(ax)],[0,sin(ax),cos(ax)]])
Roty = np.array([[cos(ay),0,sin(ay)],[0,1,0],[-sin(ay),0,cos(ay)]])
Rotz = np.array([[cos(az),-sin(az),0],[sin(az),cos(az),0],[0,0,1]])
A_cal = np.dot(A_scale,np.dot(Rotx,np.dot(Roty,Rotz)))
b = np.array(params[6:]).reshape(3,1)
display(A_cal)
display(b)
array([[ 0.98788002, -0.1216101 , 0.26669977], [ 0.05838183, 1.09541102, 0.28323532], [-0.24437697, -0.19771643, 0.81503934]])
array([[ -65.0109827 ], [-163.48151977], [ -78.73157641]])
Ниже представленные три строчки можно использовать для вставки в код
print('magx_cal = ',"%.2f" % A_cal[0,0],'*(magx + ',"%.2f" % b[0,0],') + ',"%.2f" % A_cal[0,1],'*(magy + ',"%.2f" % b[1,0],') + ',"%.2f" % A_cal[0,2],'*(magz + ',"%.2f" % b[2,0],')',sep='')
print('magy_cal = ',"%.2f" % A_cal[1,0],'*(magx + ',"%.2f" % b[0,0],') + ',"%.2f" % A_cal[1,1],'*(magy + ',"%.2f" % b[1,0],') + ',"%.2f" % A_cal[1,2],'*(magz + ',"%.2f" % b[2,0],')',sep='')
print('magz_cal = ',"%.2f" % A_cal[2,0],'*(magx + ',"%.2f" % b[0,0],') + ',"%.2f" % A_cal[2,1],'*(magy + ',"%.2f" % b[1,0],') + ',"%.2f" % A_cal[2,2],'*(magz + ',"%.2f" % b[2,0],')',sep='')
magx_cal = 0.99*(magx + -65.01) + -0.12*(magy + -163.48) + 0.27*(magz + -78.73) magy_cal = 0.06*(magx + -65.01) + 1.10*(magy + -163.48) + 0.28*(magz + -78.73) magz_cal = -0.24*(magx + -65.01) + -0.20*(magy + -163.48) + 0.82*(magz + -78.73)
# points_calibrated = np.dot(A_cal,points_filtered.transpose()).transpose()+b.transpose()
points_calibrated = np.dot(A_cal,(points_filtered+b.transpose()).transpose()).transpose()
x, y, z = [points_calibrated[:,i] for i in [0,1,2]]
#средние значения по x, y, z
xmean = np.mean(x)
ymean = np.mean(y)
zmean = np.mean(z)
#средний радиус сферы
rmean = np.mean(np.sqrt((x-xmean)**2+(y-ymean)**2+(z-zmean)**2))
sc_color = 'rgb(250, 250, 250)'
show_graphs([scatter_graph(points_calibrated,sc_color),sphere_graph(0,0,0,rmean)])
Для того чтобы не просто посмотреть содержимое методички, а выполнить ее код, необходимо установить Anaconda Navigator для запуска Jupiter Notebook (iPython). Внимание! Необходимо выбрать версию не ниже 3.6