将特殊数学函数从 matlab 移植到 numpy
Porting special math functions from matlab to numpy
这是在 MathWorks 社区部分发布的 Matlab 代码。
[x,y,z] = ellipsoid(0,-0.5,0,6,3.25,3.25);
A=[x(:),y(:),z(:)];
[U, c] = MgnCalibration(A)
function [U,c] = MgnCalibration(X)
% performs magnetometer calibration from a set of data
% using Merayo technique with a non iterative algoritm
% J.Merayo et al. "Scalar calibration of vector magnemoters"
% Meas. Sci. Technol. 11 (2000) 120-132.
%
% X : a Nx3 (or 3xN) data matrix
% each row (columns) contains x, y, z measurements
% N must be such that the data set describes
% as completely as possible the 3D space
% In any case N > 10
%
% The calibration tries to find the best 3D ellipsoid that fits the data set
% and returns the parameters of this ellipsoid
%
% U : shape ellipsoid parameter, (3x3) upper triangular matrix
% c : ellipsoid center, (3x1) vector
%
% Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
% with v a rough triaxes magnetometer measurement
%
% calibrated measurement w = U*(v-c)
%
% author : Alain Barraud, Suzanne Lesecq 2008
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[N,m] = size(X);
if m>3&&N==3,X = X';N = m;m = 3;end;%check that X is not transposed
if N<=10,U = [];c = [];return;end;%not enough data no calibration !!
% write the ellipsoid equation as D*p=0
% the best parameter is the solution of min||D*p|| with ||p||=1;
% form D matrix from X measurements
x = X(:,1); y = X(:,2); z = X(:,3);
D = [x.^2, y.^2, z.^2, x.*y, x.*z, y.*z, x, y, z, ones(N,1)];
size(D)
D=triu(qr(D));%avoids to compute the svd of a large matrix
[U,S,V] = svd(D);%because usually N may be very large
p = V(:,end);if p(1)<0,p =-p;end;
% the following matrix A(p) must be positive definite
% The optimization done by svd does not include such a constraint
% With "good" data the constraint is allways satisfied
% With too poor data A may fail to be positive definite
% In this case the calibration fails
%
A = [p(1) p(4)/2 p(5)/2;
p(4)/2 p(2) p(6)/2;
p(5)/2 p(6)/2 p(3)];
[U,ok] = fchol(m,A);
if ~ok,U = [];c = [];return;end%calibration fails too poor data!!
b = [p(7);p(8);p(9)];
v = Utsolve(U,b/2,m);
d = p(10);
s = 1/sqrt(v*v'-d);
c =-Usolve(U,v,m)';%ellipsoid center
U = s*U;%shape ellipsoid parameter
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [A,ok] = fchol(n,A)
% performs Cholesky factoristation
A(1,1:n) = A(1,1:n)/sqrt(A(1,1));
A(2:n,1) = 0;
for j=2:n
A(j,j:n) = A(j,j:n) - A(1:j-1,j)'*A(1:j-1,j:n);
if A(j,j)<=0,ok=0;break;end%A is not positive definite
A(j,j:n) = A(j,j:n)/sqrt(A(j,j));
A(j+1:n,j) = 0;
end
ok=1;
end
function x=Utsolve(U,b,n)
% solves U'*x=b
x(1) = b(1)/U(1,1);
for k=2:n
x(k) = (b(k)-x(1:k-1)*U(1:k-1,k))/U(k,k);
end
end
function x=Usolve(U,b,n)
% solves U*x=b
x(n) = b(n)/U(n,n);
for k=n-1:-1:1
x(k) = (b(k)-U(k,k+1:n)*x(k+1:n)')/U(k,k);
end
end
这是我对 NumPy 的移植，以及相同的测试数据，大部分移植工作已经完成。但是，现在我 运行 进入更高级的数学函数，如 qr、svd 和 triu 我没有经验，也没有更深刻的理解。我目前的问题是，在 numpy 方面，当使用调试器查看时，D 的各个元素按行组织并且仍然包含在 np.array() 调用中，而 Matlab 将它们排列在列中。在计算 qr() 或这两个实体时，无论我是否在 numpy 侧转置 D，结果都会不同。
这是为什么？
我已经能够验证 utsolve 和 usolve 函数了。
import numpy as np
from scipy.linalg import qr, svd
def calibrate_magnetometer(calibration_data: np.ndarray):
"""
performs magnetometer calibration from a set of data
using Merayo technique with a non iterative algoritm
J.Merayo et al. "Scalar calibration of vector magnemoters"
Meas. Sci. Technol. 11 (2000) 120-132.
X : a Nx3 (or 3xN) data matrix
each row (columns) contains x, y, z measurements
N must be such that the data set describes
as completely as possible the 3D space
In any case N > 10
The calibration tries to find the best 3D ellipsoid that fits the data set
and returns the parameters of this ellipsoid
U : shape ellipsoid parameter, (3x3) upper triangular matrix
c : ellipsoid center, (3x1) vector
Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
with v a rough triaxes magnetometer measurement
calibrated measurement w = U*(v-c)
author : Alain Barraud, Suzanne Lesecq 2008
"""
(n, m) = calibration_data.shape
if m > 3 and n == 3: # check that x is not transposed
calibration_data = calibration_data.T
n = m
m = 3
if n <= 10:
return False # not enough data for calibration
# write the ellipsoid equation as D*p=0
# the best parameter is the solution of min||D*p|| with ||p||=1
# form D matix from X measurements
x = calibration_data[:, 0]
y = calibration_data[:, 1]
z = calibration_data[:, 2]
D = np.array([np.square(x), np.square(y), np.square(z),
np.multiply(x, y), np.multiply(x, z), np.multiply(x, z),
x, y, z,
np.ones(n)])
D = np.triu(qr(D.T)) # avoids to compute the svd of a large matrix
u, s, v = svd(D) # because usually N may be very large
p = v[-1]
if p[0] < 0:
p = -p
# the following matrix a(p) must be positive definite.
# the optimization done by svd does not include such a constrain
# with "good" data the constrain is allways satisfied
# with too poor data A may fail to be positive definite
# in this case the calibration fails
a = np.array([[p[0], p[3] / 2, p[4] / 2],
[p[3] / 2, p[1], p[5] / 2],
[p[4] / 2, p[5] / 2, p[2]]])
uu = np.linalg.cholesky(a)
b = np.array([p[6], p[7], p[8]])
vv = utsolve(uu, b / 2, m)
d = p(10)
s = 1 / np.sqrt(vv * vv.T - d)
calibrated_c = -usolve(uu, vv, m) # ellipsoid center, or offset error
calibrated_u = s * uu # shape ellipsoid parameter
return calibrated_u, calibrated_c
def utsolve(u, b, n):
# solves u.T*x=b
x = np.zeros(n)
x[0] = b[0] / u[0, 0]
for k in range(1, n):
x[k] = (b[k] - np.matmul(x[0:k], u[0:k, k])) / u[k, k]
return x
def usolve(u, b, n):
# solves u*x=b
x = np.zeros((n, 1))
x[n - 1] = b[n - 1] / u[n - 1, n - 1]
for k in reversed(range(0, n - 1)):
x[k] = (b[k] - np.matmul(u[k, k + 1:n], x[k + 1:n].T[0])) / u[k, k]
return x.T[0]
def main():
a = np.array([[0, -0.5000, -3.2500],
[-0.9386, -0.5000, -3.2100],
[-1.8541, -0.5000, -3.0909],
[-2.7239, -0.5000, -2.8958],
[-3.5267, -0.5000, -2.6293],
[-4.2426, -0.5000, -2.2981],
[-4.8541, -0.5000, -1.9103],
[-5.3460, -0.5000, -1.4755],
[-5.7063, -0.5000, -1.0043],
[-5.9261, -0.5000, -0.5084],
[-6.0000, -0.5000, 0],
[-5.9261, -0.5000, 0.5084],
[-5.7063, -0.5000, 1.0043],
[-5.3460, -0.5000, 1.4755],
[-4.8541, -0.5000, 1.9103],
[-4.2426, -0.5000, 2.2981],
[-3.5267, -0.5000, 2.6293],
[-2.7239, -0.5000, 2.8958],
[-1.8541, -0.5000, 3.0909],
[-0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.8927, -0.6571, -3.2100],
[-1.7634, -0.8103, -3.0909],
[-2.5906, -0.9559, -2.8958],
[-3.3541, -1.0903, -2.6293],
[-4.0350, -1.2102, -2.2981],
[-4.6165, -1.3125, -1.9103],
[-5.0844, -1.3948, -1.4755],
[-5.4271, -1.4552, -1.0043],
[-5.6361, -1.4919, -0.5084],
[-5.7063, -1.5043, 0],
[-5.6361, -1.4919, 0.5084],
[-5.4271, -1.4552, 1.0043],
[-5.0844, -1.3948, 1.4755],
[-4.6165, -1.3125, 1.9103],
[-4.0350, -1.2102, 2.2981],
[-3.3541, -1.0903, 2.6293],
[-2.5906, -0.9559, 2.8958],
[-1.7634, -0.8103, 3.0909],
[-0.8927, -0.6571, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.7593, -0.7988, -3.2100],
[-1.5000, -1.0903, -3.0909],
[-2.2037, -1.3673, -2.8958],
[-2.8532, -1.6228, -2.6293],
[-3.4324, -1.8508, -2.2981],
[-3.9271, -2.0455, -1.9103],
[-4.3250, -2.2021, -1.4755],
[-4.6165, -2.3168, -1.0043],
[-4.7943, -2.3868, -0.5084],
[-4.8541, -2.4103, 0],
[-4.7943, -2.3868, 0.5084],
[-4.6165, -2.3168, 1.0043],
[-4.3250, -2.2021, 1.4755],
[-3.9271, -2.0455, 1.9103],
[-3.4324, -1.8508, 2.2981],
[-2.8532, -1.6228, 2.6293],
[-2.2037, -1.3673, 2.8958],
[-1.5000, -1.0903, 3.0909],
[-0.7593, -0.7988, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.5517, -0.9113, -3.2100],
[-1.0898, -1.3125, -3.0909],
[-1.6011, -1.6937, -2.8958],
[-2.0729, -2.0455, -2.6293],
[-2.4938, -2.3592, -2.2981],
[-2.8532, -2.6272, -1.9103],
[-3.1423, -2.8427, -1.4755],
[-3.3541, -3.0006, -1.0043],
[-3.4833, -3.0969, -0.5084],
[-3.5267, -3.1293, 0],
[-3.4833, -3.0969, 0.5084],
[-3.3541, -3.0006, 1.0043],
[-3.1423, -2.8427, 1.4755],
[-2.8532, -2.6272, 1.9103],
[-2.4938, -2.3592, 2.2981],
[-2.0729, -2.0455, 2.6293],
[-1.6011, -1.6937, 2.8958],
[-1.0898, -1.3125, 3.0909],
[-0.5517, -0.9113, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.2900, -0.9835, -3.2100],
[-0.5729, -1.4552, -3.0909],
[-0.8417, -1.9033, -2.8958],
[-1.0898, -2.3168, -2.6293],
[-1.3110, -2.6856, -2.2981],
[-1.5000, -3.0006, -1.9103],
[-1.6520, -3.2540, -1.4755],
[-1.7634, -3.4397, -1.0043],
[-1.8313, -3.5529, -0.5084],
[-1.8541, -3.5909, 0],
[-1.8313, -3.5529, 0.5084],
[-1.7634, -3.4397, 1.0043],
[-1.6520, -3.2540, 1.4755],
[-1.5000, -3.0006, 1.9103],
[-1.3110, -2.6856, 2.2981],
[-1.0898, -2.3168, 2.6293],
[-0.8417, -1.9033, 2.8958],
[-0.5729, -1.4552, 3.0909],
[-0.2900, -0.9835, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.0000, -1.0084, -3.2100],
[0.0000, -1.5043, -3.0909],
[0.0000, -1.9755, -2.8958],
[0.0000, -2.4103, -2.6293],
[0.0000, -2.7981, -2.2981],
[0.0000, -3.1293, -1.9103],
[0.0000, -3.3958, -1.4755],
[0.0000, -3.5909, -1.0043],
[0.0000, -3.7100, -0.5084],
[0.0000, -3.7500, 0],
[0.0000, -3.7100, 0.5084],
[0.0000, -3.5909, 1.0043],
[0.0000, -3.3958, 1.4755],
[0.0000, -3.1293, 1.9103],
[0.0000, -2.7981, 2.2981],
[0.0000, -2.4103, 2.6293],
[0.0000, -1.9755, 2.8958],
[0.0000, -1.5043, 3.0909],
[0.0000, -1.0084, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.2900, -0.9835, -3.2100],
[0.5729, -1.4552, -3.0909],
[0.8417, -1.9033, -2.8958],
[1.0898, -2.3168, -2.6293],
[1.3110, -2.6856, -2.2981],
[1.5000, -3.0006, -1.9103],
[1.6520, -3.2540, -1.4755],
[1.7634, -3.4397, -1.0043],
[1.8313, -3.5529, -0.5084],
[1.8541, -3.5909, 0],
[1.8313, -3.5529, 0.5084],
[1.7634, -3.4397, 1.0043],
[1.6520, -3.2540, 1.4755],
[1.5000, -3.0006, 1.9103],
[1.3110, -2.6856, 2.2981],
[1.0898, -2.3168, 2.6293],
[0.8417, -1.9033, 2.8958],
[0.5729, -1.4552, 3.0909],
[0.2900, -0.9835, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.5517, -0.9113, -3.2100],
[1.0898, -1.3125, -3.0909],
[1.6011, -1.6937, -2.8958],
[2.0729, -2.0455, -2.6293],
[2.4938, -2.3592, -2.2981],
[2.8532, -2.6272, -1.9103],
[3.1423, -2.8427, -1.4755],
[3.3541, -3.0006, -1.0043],
[3.4833, -3.0969, -0.5084],
[3.5267, -3.1293, 0],
[3.4833, -3.0969, 0.5084],
[3.3541, -3.0006, 1.0043],
[3.1423, -2.8427, 1.4755],
[2.8532, -2.6272, 1.9103],
[2.4938, -2.3592, 2.2981],
[2.0729, -2.0455, 2.6293],
[1.6011, -1.6937, 2.8958],
[1.0898, -1.3125, 3.0909],
[0.5517, -0.9113, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.7593, -0.7988, -3.2100],
[1.5000, -1.0903, -3.0909],
[2.2037, -1.3673, -2.8958],
[2.8532, -1.6228, -2.6293],
[3.4324, -1.8508, -2.2981],
[3.9271, -2.0455, -1.9103],
[4.3250, -2.2021, -1.4755],
[4.6165, -2.3168, -1.0043],
[4.7943, -2.3868, -0.5084],
[4.8541, -2.4103, 0],
[4.7943, -2.3868, 0.5084],
[4.6165, -2.3168, 1.0043],
[4.3250, -2.2021, 1.4755],
[3.9271, -2.0455, 1.9103],
[3.4324, -1.8508, 2.2981],
[2.8532, -1.6228, 2.6293],
[2.2037, -1.3673, 2.8958],
[1.5000, -1.0903, 3.0909],
[0.7593, -0.7988, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.8927, -0.6571, -3.2100],
[1.7634, -0.8103, -3.0909],
[2.5906, -0.9559, -2.8958],
[3.3541, -1.0903, -2.6293],
[4.0350, -1.2102, -2.2981],
[4.6165, -1.3125, -1.9103],
[5.0844, -1.3948, -1.4755],
[5.4271, -1.4552, -1.0043],
[5.6361, -1.4919, -0.5084],
[5.7063, -1.5043, 0],
[5.6361, -1.4919, 0.5084],
[5.4271, -1.4552, 1.0043],
[5.0844, -1.3948, 1.4755],
[4.6165, -1.3125, 1.9103],
[4.0350, -1.2102, 2.2981],
[3.3541, -1.0903, 2.6293],
[2.5906, -0.9559, 2.8958],
[1.7634, -0.8103, 3.0909],
[0.8927, -0.6571, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.9386, -0.5000, -3.2100],
[1.8541, -0.5000, -3.0909],
[2.7239, -0.5000, -2.8958],
[3.5267, -0.5000, -2.6293],
[4.2426, -0.5000, -2.2981],
[4.8541, -0.5000, -1.9103],
[5.3460, -0.5000, -1.4755],
[5.7063, -0.5000, -1.0043],
[5.9261, -0.5000, -0.5084],
[6.0000, -0.5000, 0],
[5.9261, -0.5000, 0.5084],
[5.7063, -0.5000, 1.0043],
[5.3460, -0.5000, 1.4755],
[4.8541, -0.5000, 1.9103],
[4.2426, -0.5000, 2.2981],
[3.5267, -0.5000, 2.6293],
[2.7239, -0.5000, 2.8958],
[1.8541, -0.5000, 3.0909],
[0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.8927, -0.3429, -3.2100],
[1.7634, -0.1897, -3.0909],
[2.5906, -0.0441, -2.8958],
[3.3541, 0.0903, -2.6293],
[4.0350, 0.2102, -2.2981],
[4.6165, 0.3125, -1.9103],
[5.0844, 0.3948, -1.4755],
[5.4271, 0.4552, -1.0043],
[5.6361, 0.4919, -0.5084],
[5.7063, 0.5043, 0],
[5.6361, 0.4919, 0.5084],
[5.4271, 0.4552, 1.0043],
[5.0844, 0.3948, 1.4755],
[4.6165, 0.3125, 1.9103],
[4.0350, 0.2102, 2.2981],
[3.3541, 0.0903, 2.6293],
[2.5906, -0.0441, 2.8958],
[1.7634, -0.1897, 3.0909],
[0.8927, -0.3429, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.7593, -0.2012, -3.2100],
[1.5000, 0.0903, -3.0909],
[2.2037, 0.3673, -2.8958],
[2.8532, 0.6228, -2.6293],
[3.4324, 0.8508, -2.2981],
[3.9271, 1.0455, -1.9103],
[4.3250, 1.2021, -1.4755],
[4.6165, 1.3168, -1.0043],
[4.7943, 1.3868, -0.5084],
[4.8541, 1.4103, 0],
[4.7943, 1.3868, 0.5084],
[4.6165, 1.3168, 1.0043],
[4.3250, 1.2021, 1.4755],
[3.9271, 1.0455, 1.9103],
[3.4324, 0.8508, 2.2981],
[2.8532, 0.6228, 2.6293],
[2.2037, 0.3673, 2.8958],
[1.5000, 0.0903, 3.0909],
[0.7593, -0.2012, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.5517, -0.0887, -3.2100],
[1.0898, 0.3125, -3.0909],
[1.6011, 0.6937, -2.8958],
[2.0729, 1.0455, -2.6293],
[2.4938, 1.3592, -2.2981],
[2.8532, 1.6272, -1.9103],
[3.1423, 1.8427, -1.4755],
[3.3541, 2.0006, -1.0043],
[3.4833, 2.0969, -0.5084],
[3.5267, 2.1293, 0],
[3.4833, 2.0969, 0.5084],
[3.3541, 2.0006, 1.0043],
[3.1423, 1.8427, 1.4755],
[2.8532, 1.6272, 1.9103],
[2.4938, 1.3592, 2.2981],
[2.0729, 1.0455, 2.6293],
[1.6011, 0.6937, 2.8958],
[1.0898, 0.3125, 3.0909],
[0.5517, -0.0887, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.2900, -0.0165, -3.2100],
[0.5729, 0.4552, -3.0909],
[0.8417, 0.9033, -2.8958],
[1.0898, 1.3168, -2.6293],
[1.3110, 1.6856, -2.2981],
[1.5000, 2.0006, -1.9103],
[1.6520, 2.2540, -1.4755],
[1.7634, 2.4397, -1.0043],
[1.8313, 2.5529, -0.5084],
[1.8541, 2.5909, 0],
[1.8313, 2.5529, 0.5084],
[1.7634, 2.4397, 1.0043],
[1.6520, 2.2540, 1.4755],
[1.5000, 2.0006, 1.9103],
[1.3110, 1.6856, 2.2981],
[1.0898, 1.3168, 2.6293],
[0.8417, 0.9033, 2.8958],
[0.5729, 0.4552, 3.0909],
[0.2900, -0.0165, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.0000, 0.0084, -3.2100],
[0.0000, 0.5043, -3.0909],
[0.0000, 0.9755, -2.8958],
[0.0000, 1.4103, -2.6293],
[0.0000, 1.7981, -2.2981],
[0.0000, 2.1293, -1.9103],
[0.0000, 2.3958, -1.4755],
[0.0000, 2.5909, -1.0043],
[0.0000, 2.7100, -0.5084],
[0.0000, 2.7500, 0],
[0.0000, 2.7100, 0.5084],
[0.0000, 2.5909, 1.0043],
[0.0000, 2.3958, 1.4755],
[0.0000, 2.1293, 1.9103],
[0.0000, 1.7981, 2.2981],
[0.0000, 1.4103, 2.6293],
[0.0000, 0.9755, 2.8958],
[0.0000, 0.5043, 3.0909],
[0.0000, 0.0084, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.2900, -0.0165, -3.2100],
[-0.5729, 0.4552, -3.0909],
[-0.8417, 0.9033, -2.8958],
[-1.0898, 1.3168, -2.6293],
[-1.3110, 1.6856, -2.2981],
[-1.5000, 2.0006, -1.9103],
[-1.6520, 2.2540, -1.4755],
[-1.7634, 2.4397, -1.0043],
[-1.8313, 2.5529, -0.5084],
[-1.8541, 2.5909, 0],
[-1.8313, 2.5529, 0.5084],
[-1.7634, 2.4397, 1.0043],
[-1.6520, 2.2540, 1.4755],
[-1.5000, 2.0006, 1.9103],
[-1.3110, 1.6856, 2.2981],
[-1.0898, 1.3168, 2.6293],
[-0.8417, 0.9033, 2.8958],
[-0.5729, 0.4552, 3.0909],
[-0.2900, -0.0165, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.5517, -0.0887, -3.2100],
[-1.0898, 0.3125, -3.0909],
[-1.6011, 0.6937, -2.8958],
[-2.0729, 1.0455, -2.6293],
[-2.4938, 1.3592, -2.2981],
[-2.8532, 1.6272, -1.9103],
[-3.1423, 1.8427, -1.4755],
[-3.3541, 2.0006, -1.0043],
[-3.4833, 2.0969, -0.5084],
[-3.5267, 2.1293, 0],
[-3.4833, 2.0969, 0.5084],
[-3.3541, 2.0006, 1.0043],
[-3.1423, 1.8427, 1.4755],
[-2.8532, 1.6272, 1.9103],
[-2.4938, 1.3592, 2.2981],
[-2.0729, 1.0455, 2.6293],
[-1.6011, 0.6937, 2.8958],
[-1.0898, 0.3125, 3.0909],
[-0.5517, -0.0887, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.7593, -0.2012, -3.2100],
[-1.5000, 0.0903, -3.0909],
[-2.2037, 0.3673, -2.8958],
[-2.8532, 0.6228, -2.6293],
[-3.4324, 0.8508, -2.2981],
[-3.9271, 1.0455, -1.9103],
[-4.3250, 1.2021, -1.4755],
[-4.6165, 1.3168, -1.0043],
[-4.7943, 1.3868, -0.5084],
[-4.8541, 1.4103, 0],
[-4.7943, 1.3868, 0.5084],
[-4.6165, 1.3168, 1.0043],
[-4.3250, 1.2021, 1.4755],
[-3.9271, 1.0455, 1.9103],
[-3.4324, 0.8508, 2.2981],
[-2.8532, 0.6228, 2.6293],
[-2.2037, 0.3673, 2.8958],
[-1.5000, 0.0903, 3.0909],
[-0.7593, -0.2012, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.8927, -0.3429, -3.2100],
[-1.7634, -0.1897, -3.0909],
[-2.5906, -0.0441, -2.8958],
[-3.3541, 0.0903, -2.6293],
[-4.0350, 0.2102, -2.2981],
[-4.6165, 0.3125, -1.9103],
[-5.0844, 0.3948, -1.4755],
[-5.4271, 0.4552, -1.0043],
[-5.6361, 0.4919, -0.5084],
[-5.7063, 0.5043, 0],
[-5.6361, 0.4919, 0.5084],
[-5.4271, 0.4552, 1.0043],
[-5.0844, 0.3948, 1.4755],
[-4.6165, 0.3125, 1.9103],
[-4.0350, 0.2102, 2.2981],
[-3.3541, 0.0903, 2.6293],
[-2.5906, -0.0441, 2.8958],
[-1.7634, -0.1897, 3.0909],
[-0.8927, -0.3429, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.9386, -0.5000, -3.2100],
[-1.8541, -0.5000, -3.0909],
[-2.7239, -0.5000, -2.8958],
[-3.5267, -0.5000, -2.6293],
[-4.2426, -0.5000, -2.2981],
[-4.8541, -0.5000, -1.9103],
[-5.3460, -0.5000, -1.4755],
[-5.7063, -0.5000, -1.0043],
[-5.9261, -0.5000, -0.5084],
[-6.0000, -0.5000, 0],
[-5.9261, -0.5000, 0.5084],
[-5.7063, -0.5000, 1.0043],
[-5.3460, -0.5000, 1.4755],
[-4.8541, -0.5000, 1.9103],
[-4.2426, -0.5000, 2.2981],
[-3.5267, -0.5000, 2.6293],
[-2.7239, -0.5000, 2.8958],
[-1.8541, -0.5000, 3.0909],
[-0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500]])
u = np.arange(1, 17).reshape(4, -1)
b = np.arange(4)
print("utsolve: {}".format(calibrate_magnetometer(a)))
if __name__ == "__main__":
main()
这似乎像 matlab 代码一样工作。剩下的问题主要是“扁平化”D 中的内部 np.arrays 和 select 来自 qr() 的第二个返回值。
import numpy as np
from scipy.linalg import qr, svd
def calibrate_magnetometer(calibration_data: np.ndarray):
"""
performs magnetometer calibration from a set of data
using Merayo technique with a non iterative algoritm
J.Merayo et al. "Scalar calibration of vector magnemoters"
Meas. Sci. Technol. 11 (2000) 120-132.
X : a Nx3 (or 3xN) data matrix
each row (columns) contains x, y, z measurements
N must be such that the data set describes
as completely as possible the 3D space
In any case N > 10
The calibration tries to find the best 3D ellipsoid that fits the data set
and returns the parameters of this ellipsoid
U : shape ellipsoid parameter, (3x3) upper triangular matrix
c : ellipsoid center, (3x1) vector
Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
with v a rough triaxes magnetometer measurement
calibrated measurement w = U*(v-c)
author : Alain Barraud, Suzanne Lesecq 2008
"""
(n, m) = calibration_data.shape
if m > 3 and n == 3: # check that x is not transposed
calibration_data = calibration_data.T
n = m
m = 3
if n <= 10:
return False # not enough data for calibration
# write the ellipsoid equation as D*p=0
# the best parameter is the solution of min||D*p|| with ||p||=1
# form D matix from X measurements
x = calibration_data[:, 0]
y = calibration_data[:, 1]
z = calibration_data[:, 2]
D = [np.square(x), np.square(y), np.square(z),
np.multiply(x,y), np.multiply(x,z), np.multiply(y,z),
x, y, z,
np.ones(n)]
D = np.array(D)
D = np.triu(qr(D.T)[1]) # avoids to compute the svd of a large matrix
u, s, v = svd(D) # because usually N may be very large
p = v[-1]
if p[0] < 0:
p = -p
# the following matrix a(p) must be positive definite.
# the optimization done by svd does not include such a constrain
# with "good" data the constrain is allways satisfied
# with too poor data A may fail to be positive definite
# in this case the calibration fails
a = np.array([[p[0], p[3] / 2, p[4] / 2],
[p[3] / 2, p[1], p[5] / 2],
[p[4] / 2, p[5] / 2, p[2]]])
uu = np.linalg.cholesky(a)
b = np.array([p[6], p[7], p[8]])
vv = utsolve(uu, b / 2, m)
d = p[9]
s = 1 / np.sqrt(vv * vv.T - d)
calibrated_c = -usolve(uu, vv, m) # ellipsoid center, or offset error
calibrated_u = s * uu # shape ellipsoid parameter
return calibrated_u, calibrated_c
def utsolve(u, b, n):
# solves u.T*x=b
x = np.zeros(n)
x[0] = b[0] / u[0, 0]
for k in range(1, n):
x[k] = (b[k] - np.matmul(x[0:k], u[0:k, k])) / u[k, k]
return x
def usolve(u, b, n):
# solves u*x=b
x = np.zeros((n, 1))
x[n - 1] = b[n - 1] / u[n - 1, n - 1]
for k in reversed(range(0, n - 1)):
x[k] = (b[k] - np.matmul(u[k, k + 1:n], x[k + 1:n].T[0])) / u[k, k]
return x.T[0]
这是在 MathWorks 社区部分发布的 Matlab 代码。
[x,y,z] = ellipsoid(0,-0.5,0,6,3.25,3.25);
A=[x(:),y(:),z(:)];
[U, c] = MgnCalibration(A)
function [U,c] = MgnCalibration(X)
% performs magnetometer calibration from a set of data
% using Merayo technique with a non iterative algoritm
% J.Merayo et al. "Scalar calibration of vector magnemoters"
% Meas. Sci. Technol. 11 (2000) 120-132.
%
% X : a Nx3 (or 3xN) data matrix
% each row (columns) contains x, y, z measurements
% N must be such that the data set describes
% as completely as possible the 3D space
% In any case N > 10
%
% The calibration tries to find the best 3D ellipsoid that fits the data set
% and returns the parameters of this ellipsoid
%
% U : shape ellipsoid parameter, (3x3) upper triangular matrix
% c : ellipsoid center, (3x1) vector
%
% Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
% with v a rough triaxes magnetometer measurement
%
% calibrated measurement w = U*(v-c)
%
% author : Alain Barraud, Suzanne Lesecq 2008
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[N,m] = size(X);
if m>3&&N==3,X = X';N = m;m = 3;end;%check that X is not transposed
if N<=10,U = [];c = [];return;end;%not enough data no calibration !!
% write the ellipsoid equation as D*p=0
% the best parameter is the solution of min||D*p|| with ||p||=1;
% form D matrix from X measurements
x = X(:,1); y = X(:,2); z = X(:,3);
D = [x.^2, y.^2, z.^2, x.*y, x.*z, y.*z, x, y, z, ones(N,1)];
size(D)
D=triu(qr(D));%avoids to compute the svd of a large matrix
[U,S,V] = svd(D);%because usually N may be very large
p = V(:,end);if p(1)<0,p =-p;end;
% the following matrix A(p) must be positive definite
% The optimization done by svd does not include such a constraint
% With "good" data the constraint is allways satisfied
% With too poor data A may fail to be positive definite
% In this case the calibration fails
%
A = [p(1) p(4)/2 p(5)/2;
p(4)/2 p(2) p(6)/2;
p(5)/2 p(6)/2 p(3)];
[U,ok] = fchol(m,A);
if ~ok,U = [];c = [];return;end%calibration fails too poor data!!
b = [p(7);p(8);p(9)];
v = Utsolve(U,b/2,m);
d = p(10);
s = 1/sqrt(v*v'-d);
c =-Usolve(U,v,m)';%ellipsoid center
U = s*U;%shape ellipsoid parameter
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [A,ok] = fchol(n,A)
% performs Cholesky factoristation
A(1,1:n) = A(1,1:n)/sqrt(A(1,1));
A(2:n,1) = 0;
for j=2:n
A(j,j:n) = A(j,j:n) - A(1:j-1,j)'*A(1:j-1,j:n);
if A(j,j)<=0,ok=0;break;end%A is not positive definite
A(j,j:n) = A(j,j:n)/sqrt(A(j,j));
A(j+1:n,j) = 0;
end
ok=1;
end
function x=Utsolve(U,b,n)
% solves U'*x=b
x(1) = b(1)/U(1,1);
for k=2:n
x(k) = (b(k)-x(1:k-1)*U(1:k-1,k))/U(k,k);
end
end
function x=Usolve(U,b,n)
% solves U*x=b
x(n) = b(n)/U(n,n);
for k=n-1:-1:1
x(k) = (b(k)-U(k,k+1:n)*x(k+1:n)')/U(k,k);
end
end
这是我对 NumPy 的移植，以及相同的测试数据，大部分移植工作已经完成。但是，现在我 运行 进入更高级的数学函数，如 qr、svd 和 triu 我没有经验，也没有更深刻的理解。我目前的问题是，在 numpy 方面，当使用调试器查看时，D 的各个元素按行组织并且仍然包含在 np.array() 调用中，而 Matlab 将它们排列在列中。在计算 qr() 或这两个实体时，无论我是否在 numpy 侧转置 D，结果都会不同。
这是为什么？
我已经能够验证 utsolve 和 usolve 函数了。
import numpy as np
from scipy.linalg import qr, svd
def calibrate_magnetometer(calibration_data: np.ndarray):
"""
performs magnetometer calibration from a set of data
using Merayo technique with a non iterative algoritm
J.Merayo et al. "Scalar calibration of vector magnemoters"
Meas. Sci. Technol. 11 (2000) 120-132.
X : a Nx3 (or 3xN) data matrix
each row (columns) contains x, y, z measurements
N must be such that the data set describes
as completely as possible the 3D space
In any case N > 10
The calibration tries to find the best 3D ellipsoid that fits the data set
and returns the parameters of this ellipsoid
U : shape ellipsoid parameter, (3x3) upper triangular matrix
c : ellipsoid center, (3x1) vector
Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
with v a rough triaxes magnetometer measurement
calibrated measurement w = U*(v-c)
author : Alain Barraud, Suzanne Lesecq 2008
"""
(n, m) = calibration_data.shape
if m > 3 and n == 3: # check that x is not transposed
calibration_data = calibration_data.T
n = m
m = 3
if n <= 10:
return False # not enough data for calibration
# write the ellipsoid equation as D*p=0
# the best parameter is the solution of min||D*p|| with ||p||=1
# form D matix from X measurements
x = calibration_data[:, 0]
y = calibration_data[:, 1]
z = calibration_data[:, 2]
D = np.array([np.square(x), np.square(y), np.square(z),
np.multiply(x, y), np.multiply(x, z), np.multiply(x, z),
x, y, z,
np.ones(n)])
D = np.triu(qr(D.T)) # avoids to compute the svd of a large matrix
u, s, v = svd(D) # because usually N may be very large
p = v[-1]
if p[0] < 0:
p = -p
# the following matrix a(p) must be positive definite.
# the optimization done by svd does not include such a constrain
# with "good" data the constrain is allways satisfied
# with too poor data A may fail to be positive definite
# in this case the calibration fails
a = np.array([[p[0], p[3] / 2, p[4] / 2],
[p[3] / 2, p[1], p[5] / 2],
[p[4] / 2, p[5] / 2, p[2]]])
uu = np.linalg.cholesky(a)
b = np.array([p[6], p[7], p[8]])
vv = utsolve(uu, b / 2, m)
d = p(10)
s = 1 / np.sqrt(vv * vv.T - d)
calibrated_c = -usolve(uu, vv, m) # ellipsoid center, or offset error
calibrated_u = s * uu # shape ellipsoid parameter
return calibrated_u, calibrated_c
def utsolve(u, b, n):
# solves u.T*x=b
x = np.zeros(n)
x[0] = b[0] / u[0, 0]
for k in range(1, n):
x[k] = (b[k] - np.matmul(x[0:k], u[0:k, k])) / u[k, k]
return x
def usolve(u, b, n):
# solves u*x=b
x = np.zeros((n, 1))
x[n - 1] = b[n - 1] / u[n - 1, n - 1]
for k in reversed(range(0, n - 1)):
x[k] = (b[k] - np.matmul(u[k, k + 1:n], x[k + 1:n].T[0])) / u[k, k]
return x.T[0]
def main():
a = np.array([[0, -0.5000, -3.2500],
[-0.9386, -0.5000, -3.2100],
[-1.8541, -0.5000, -3.0909],
[-2.7239, -0.5000, -2.8958],
[-3.5267, -0.5000, -2.6293],
[-4.2426, -0.5000, -2.2981],
[-4.8541, -0.5000, -1.9103],
[-5.3460, -0.5000, -1.4755],
[-5.7063, -0.5000, -1.0043],
[-5.9261, -0.5000, -0.5084],
[-6.0000, -0.5000, 0],
[-5.9261, -0.5000, 0.5084],
[-5.7063, -0.5000, 1.0043],
[-5.3460, -0.5000, 1.4755],
[-4.8541, -0.5000, 1.9103],
[-4.2426, -0.5000, 2.2981],
[-3.5267, -0.5000, 2.6293],
[-2.7239, -0.5000, 2.8958],
[-1.8541, -0.5000, 3.0909],
[-0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.8927, -0.6571, -3.2100],
[-1.7634, -0.8103, -3.0909],
[-2.5906, -0.9559, -2.8958],
[-3.3541, -1.0903, -2.6293],
[-4.0350, -1.2102, -2.2981],
[-4.6165, -1.3125, -1.9103],
[-5.0844, -1.3948, -1.4755],
[-5.4271, -1.4552, -1.0043],
[-5.6361, -1.4919, -0.5084],
[-5.7063, -1.5043, 0],
[-5.6361, -1.4919, 0.5084],
[-5.4271, -1.4552, 1.0043],
[-5.0844, -1.3948, 1.4755],
[-4.6165, -1.3125, 1.9103],
[-4.0350, -1.2102, 2.2981],
[-3.3541, -1.0903, 2.6293],
[-2.5906, -0.9559, 2.8958],
[-1.7634, -0.8103, 3.0909],
[-0.8927, -0.6571, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.7593, -0.7988, -3.2100],
[-1.5000, -1.0903, -3.0909],
[-2.2037, -1.3673, -2.8958],
[-2.8532, -1.6228, -2.6293],
[-3.4324, -1.8508, -2.2981],
[-3.9271, -2.0455, -1.9103],
[-4.3250, -2.2021, -1.4755],
[-4.6165, -2.3168, -1.0043],
[-4.7943, -2.3868, -0.5084],
[-4.8541, -2.4103, 0],
[-4.7943, -2.3868, 0.5084],
[-4.6165, -2.3168, 1.0043],
[-4.3250, -2.2021, 1.4755],
[-3.9271, -2.0455, 1.9103],
[-3.4324, -1.8508, 2.2981],
[-2.8532, -1.6228, 2.6293],
[-2.2037, -1.3673, 2.8958],
[-1.5000, -1.0903, 3.0909],
[-0.7593, -0.7988, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.5517, -0.9113, -3.2100],
[-1.0898, -1.3125, -3.0909],
[-1.6011, -1.6937, -2.8958],
[-2.0729, -2.0455, -2.6293],
[-2.4938, -2.3592, -2.2981],
[-2.8532, -2.6272, -1.9103],
[-3.1423, -2.8427, -1.4755],
[-3.3541, -3.0006, -1.0043],
[-3.4833, -3.0969, -0.5084],
[-3.5267, -3.1293, 0],
[-3.4833, -3.0969, 0.5084],
[-3.3541, -3.0006, 1.0043],
[-3.1423, -2.8427, 1.4755],
[-2.8532, -2.6272, 1.9103],
[-2.4938, -2.3592, 2.2981],
[-2.0729, -2.0455, 2.6293],
[-1.6011, -1.6937, 2.8958],
[-1.0898, -1.3125, 3.0909],
[-0.5517, -0.9113, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.2900, -0.9835, -3.2100],
[-0.5729, -1.4552, -3.0909],
[-0.8417, -1.9033, -2.8958],
[-1.0898, -2.3168, -2.6293],
[-1.3110, -2.6856, -2.2981],
[-1.5000, -3.0006, -1.9103],
[-1.6520, -3.2540, -1.4755],
[-1.7634, -3.4397, -1.0043],
[-1.8313, -3.5529, -0.5084],
[-1.8541, -3.5909, 0],
[-1.8313, -3.5529, 0.5084],
[-1.7634, -3.4397, 1.0043],
[-1.6520, -3.2540, 1.4755],
[-1.5000, -3.0006, 1.9103],
[-1.3110, -2.6856, 2.2981],
[-1.0898, -2.3168, 2.6293],
[-0.8417, -1.9033, 2.8958],
[-0.5729, -1.4552, 3.0909],
[-0.2900, -0.9835, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.0000, -1.0084, -3.2100],
[0.0000, -1.5043, -3.0909],
[0.0000, -1.9755, -2.8958],
[0.0000, -2.4103, -2.6293],
[0.0000, -2.7981, -2.2981],
[0.0000, -3.1293, -1.9103],
[0.0000, -3.3958, -1.4755],
[0.0000, -3.5909, -1.0043],
[0.0000, -3.7100, -0.5084],
[0.0000, -3.7500, 0],
[0.0000, -3.7100, 0.5084],
[0.0000, -3.5909, 1.0043],
[0.0000, -3.3958, 1.4755],
[0.0000, -3.1293, 1.9103],
[0.0000, -2.7981, 2.2981],
[0.0000, -2.4103, 2.6293],
[0.0000, -1.9755, 2.8958],
[0.0000, -1.5043, 3.0909],
[0.0000, -1.0084, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.2900, -0.9835, -3.2100],
[0.5729, -1.4552, -3.0909],
[0.8417, -1.9033, -2.8958],
[1.0898, -2.3168, -2.6293],
[1.3110, -2.6856, -2.2981],
[1.5000, -3.0006, -1.9103],
[1.6520, -3.2540, -1.4755],
[1.7634, -3.4397, -1.0043],
[1.8313, -3.5529, -0.5084],
[1.8541, -3.5909, 0],
[1.8313, -3.5529, 0.5084],
[1.7634, -3.4397, 1.0043],
[1.6520, -3.2540, 1.4755],
[1.5000, -3.0006, 1.9103],
[1.3110, -2.6856, 2.2981],
[1.0898, -2.3168, 2.6293],
[0.8417, -1.9033, 2.8958],
[0.5729, -1.4552, 3.0909],
[0.2900, -0.9835, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.5517, -0.9113, -3.2100],
[1.0898, -1.3125, -3.0909],
[1.6011, -1.6937, -2.8958],
[2.0729, -2.0455, -2.6293],
[2.4938, -2.3592, -2.2981],
[2.8532, -2.6272, -1.9103],
[3.1423, -2.8427, -1.4755],
[3.3541, -3.0006, -1.0043],
[3.4833, -3.0969, -0.5084],
[3.5267, -3.1293, 0],
[3.4833, -3.0969, 0.5084],
[3.3541, -3.0006, 1.0043],
[3.1423, -2.8427, 1.4755],
[2.8532, -2.6272, 1.9103],
[2.4938, -2.3592, 2.2981],
[2.0729, -2.0455, 2.6293],
[1.6011, -1.6937, 2.8958],
[1.0898, -1.3125, 3.0909],
[0.5517, -0.9113, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.7593, -0.7988, -3.2100],
[1.5000, -1.0903, -3.0909],
[2.2037, -1.3673, -2.8958],
[2.8532, -1.6228, -2.6293],
[3.4324, -1.8508, -2.2981],
[3.9271, -2.0455, -1.9103],
[4.3250, -2.2021, -1.4755],
[4.6165, -2.3168, -1.0043],
[4.7943, -2.3868, -0.5084],
[4.8541, -2.4103, 0],
[4.7943, -2.3868, 0.5084],
[4.6165, -2.3168, 1.0043],
[4.3250, -2.2021, 1.4755],
[3.9271, -2.0455, 1.9103],
[3.4324, -1.8508, 2.2981],
[2.8532, -1.6228, 2.6293],
[2.2037, -1.3673, 2.8958],
[1.5000, -1.0903, 3.0909],
[0.7593, -0.7988, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.8927, -0.6571, -3.2100],
[1.7634, -0.8103, -3.0909],
[2.5906, -0.9559, -2.8958],
[3.3541, -1.0903, -2.6293],
[4.0350, -1.2102, -2.2981],
[4.6165, -1.3125, -1.9103],
[5.0844, -1.3948, -1.4755],
[5.4271, -1.4552, -1.0043],
[5.6361, -1.4919, -0.5084],
[5.7063, -1.5043, 0],
[5.6361, -1.4919, 0.5084],
[5.4271, -1.4552, 1.0043],
[5.0844, -1.3948, 1.4755],
[4.6165, -1.3125, 1.9103],
[4.0350, -1.2102, 2.2981],
[3.3541, -1.0903, 2.6293],
[2.5906, -0.9559, 2.8958],
[1.7634, -0.8103, 3.0909],
[0.8927, -0.6571, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.9386, -0.5000, -3.2100],
[1.8541, -0.5000, -3.0909],
[2.7239, -0.5000, -2.8958],
[3.5267, -0.5000, -2.6293],
[4.2426, -0.5000, -2.2981],
[4.8541, -0.5000, -1.9103],
[5.3460, -0.5000, -1.4755],
[5.7063, -0.5000, -1.0043],
[5.9261, -0.5000, -0.5084],
[6.0000, -0.5000, 0],
[5.9261, -0.5000, 0.5084],
[5.7063, -0.5000, 1.0043],
[5.3460, -0.5000, 1.4755],
[4.8541, -0.5000, 1.9103],
[4.2426, -0.5000, 2.2981],
[3.5267, -0.5000, 2.6293],
[2.7239, -0.5000, 2.8958],
[1.8541, -0.5000, 3.0909],
[0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.8927, -0.3429, -3.2100],
[1.7634, -0.1897, -3.0909],
[2.5906, -0.0441, -2.8958],
[3.3541, 0.0903, -2.6293],
[4.0350, 0.2102, -2.2981],
[4.6165, 0.3125, -1.9103],
[5.0844, 0.3948, -1.4755],
[5.4271, 0.4552, -1.0043],
[5.6361, 0.4919, -0.5084],
[5.7063, 0.5043, 0],
[5.6361, 0.4919, 0.5084],
[5.4271, 0.4552, 1.0043],
[5.0844, 0.3948, 1.4755],
[4.6165, 0.3125, 1.9103],
[4.0350, 0.2102, 2.2981],
[3.3541, 0.0903, 2.6293],
[2.5906, -0.0441, 2.8958],
[1.7634, -0.1897, 3.0909],
[0.8927, -0.3429, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.7593, -0.2012, -3.2100],
[1.5000, 0.0903, -3.0909],
[2.2037, 0.3673, -2.8958],
[2.8532, 0.6228, -2.6293],
[3.4324, 0.8508, -2.2981],
[3.9271, 1.0455, -1.9103],
[4.3250, 1.2021, -1.4755],
[4.6165, 1.3168, -1.0043],
[4.7943, 1.3868, -0.5084],
[4.8541, 1.4103, 0],
[4.7943, 1.3868, 0.5084],
[4.6165, 1.3168, 1.0043],
[4.3250, 1.2021, 1.4755],
[3.9271, 1.0455, 1.9103],
[3.4324, 0.8508, 2.2981],
[2.8532, 0.6228, 2.6293],
[2.2037, 0.3673, 2.8958],
[1.5000, 0.0903, 3.0909],
[0.7593, -0.2012, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.5517, -0.0887, -3.2100],
[1.0898, 0.3125, -3.0909],
[1.6011, 0.6937, -2.8958],
[2.0729, 1.0455, -2.6293],
[2.4938, 1.3592, -2.2981],
[2.8532, 1.6272, -1.9103],
[3.1423, 1.8427, -1.4755],
[3.3541, 2.0006, -1.0043],
[3.4833, 2.0969, -0.5084],
[3.5267, 2.1293, 0],
[3.4833, 2.0969, 0.5084],
[3.3541, 2.0006, 1.0043],
[3.1423, 1.8427, 1.4755],
[2.8532, 1.6272, 1.9103],
[2.4938, 1.3592, 2.2981],
[2.0729, 1.0455, 2.6293],
[1.6011, 0.6937, 2.8958],
[1.0898, 0.3125, 3.0909],
[0.5517, -0.0887, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.2900, -0.0165, -3.2100],
[0.5729, 0.4552, -3.0909],
[0.8417, 0.9033, -2.8958],
[1.0898, 1.3168, -2.6293],
[1.3110, 1.6856, -2.2981],
[1.5000, 2.0006, -1.9103],
[1.6520, 2.2540, -1.4755],
[1.7634, 2.4397, -1.0043],
[1.8313, 2.5529, -0.5084],
[1.8541, 2.5909, 0],
[1.8313, 2.5529, 0.5084],
[1.7634, 2.4397, 1.0043],
[1.6520, 2.2540, 1.4755],
[1.5000, 2.0006, 1.9103],
[1.3110, 1.6856, 2.2981],
[1.0898, 1.3168, 2.6293],
[0.8417, 0.9033, 2.8958],
[0.5729, 0.4552, 3.0909],
[0.2900, -0.0165, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[0.0000, 0.0084, -3.2100],
[0.0000, 0.5043, -3.0909],
[0.0000, 0.9755, -2.8958],
[0.0000, 1.4103, -2.6293],
[0.0000, 1.7981, -2.2981],
[0.0000, 2.1293, -1.9103],
[0.0000, 2.3958, -1.4755],
[0.0000, 2.5909, -1.0043],
[0.0000, 2.7100, -0.5084],
[0.0000, 2.7500, 0],
[0.0000, 2.7100, 0.5084],
[0.0000, 2.5909, 1.0043],
[0.0000, 2.3958, 1.4755],
[0.0000, 2.1293, 1.9103],
[0.0000, 1.7981, 2.2981],
[0.0000, 1.4103, 2.6293],
[0.0000, 0.9755, 2.8958],
[0.0000, 0.5043, 3.0909],
[0.0000, 0.0084, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.2900, -0.0165, -3.2100],
[-0.5729, 0.4552, -3.0909],
[-0.8417, 0.9033, -2.8958],
[-1.0898, 1.3168, -2.6293],
[-1.3110, 1.6856, -2.2981],
[-1.5000, 2.0006, -1.9103],
[-1.6520, 2.2540, -1.4755],
[-1.7634, 2.4397, -1.0043],
[-1.8313, 2.5529, -0.5084],
[-1.8541, 2.5909, 0],
[-1.8313, 2.5529, 0.5084],
[-1.7634, 2.4397, 1.0043],
[-1.6520, 2.2540, 1.4755],
[-1.5000, 2.0006, 1.9103],
[-1.3110, 1.6856, 2.2981],
[-1.0898, 1.3168, 2.6293],
[-0.8417, 0.9033, 2.8958],
[-0.5729, 0.4552, 3.0909],
[-0.2900, -0.0165, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.5517, -0.0887, -3.2100],
[-1.0898, 0.3125, -3.0909],
[-1.6011, 0.6937, -2.8958],
[-2.0729, 1.0455, -2.6293],
[-2.4938, 1.3592, -2.2981],
[-2.8532, 1.6272, -1.9103],
[-3.1423, 1.8427, -1.4755],
[-3.3541, 2.0006, -1.0043],
[-3.4833, 2.0969, -0.5084],
[-3.5267, 2.1293, 0],
[-3.4833, 2.0969, 0.5084],
[-3.3541, 2.0006, 1.0043],
[-3.1423, 1.8427, 1.4755],
[-2.8532, 1.6272, 1.9103],
[-2.4938, 1.3592, 2.2981],
[-2.0729, 1.0455, 2.6293],
[-1.6011, 0.6937, 2.8958],
[-1.0898, 0.3125, 3.0909],
[-0.5517, -0.0887, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.7593, -0.2012, -3.2100],
[-1.5000, 0.0903, -3.0909],
[-2.2037, 0.3673, -2.8958],
[-2.8532, 0.6228, -2.6293],
[-3.4324, 0.8508, -2.2981],
[-3.9271, 1.0455, -1.9103],
[-4.3250, 1.2021, -1.4755],
[-4.6165, 1.3168, -1.0043],
[-4.7943, 1.3868, -0.5084],
[-4.8541, 1.4103, 0],
[-4.7943, 1.3868, 0.5084],
[-4.6165, 1.3168, 1.0043],
[-4.3250, 1.2021, 1.4755],
[-3.9271, 1.0455, 1.9103],
[-3.4324, 0.8508, 2.2981],
[-2.8532, 0.6228, 2.6293],
[-2.2037, 0.3673, 2.8958],
[-1.5000, 0.0903, 3.0909],
[-0.7593, -0.2012, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.8927, -0.3429, -3.2100],
[-1.7634, -0.1897, -3.0909],
[-2.5906, -0.0441, -2.8958],
[-3.3541, 0.0903, -2.6293],
[-4.0350, 0.2102, -2.2981],
[-4.6165, 0.3125, -1.9103],
[-5.0844, 0.3948, -1.4755],
[-5.4271, 0.4552, -1.0043],
[-5.6361, 0.4919, -0.5084],
[-5.7063, 0.5043, 0],
[-5.6361, 0.4919, 0.5084],
[-5.4271, 0.4552, 1.0043],
[-5.0844, 0.3948, 1.4755],
[-4.6165, 0.3125, 1.9103],
[-4.0350, 0.2102, 2.2981],
[-3.3541, 0.0903, 2.6293],
[-2.5906, -0.0441, 2.8958],
[-1.7634, -0.1897, 3.0909],
[-0.8927, -0.3429, 3.2100],
[0, -0.5000, 3.2500],
[0, -0.5000, -3.2500],
[-0.9386, -0.5000, -3.2100],
[-1.8541, -0.5000, -3.0909],
[-2.7239, -0.5000, -2.8958],
[-3.5267, -0.5000, -2.6293],
[-4.2426, -0.5000, -2.2981],
[-4.8541, -0.5000, -1.9103],
[-5.3460, -0.5000, -1.4755],
[-5.7063, -0.5000, -1.0043],
[-5.9261, -0.5000, -0.5084],
[-6.0000, -0.5000, 0],
[-5.9261, -0.5000, 0.5084],
[-5.7063, -0.5000, 1.0043],
[-5.3460, -0.5000, 1.4755],
[-4.8541, -0.5000, 1.9103],
[-4.2426, -0.5000, 2.2981],
[-3.5267, -0.5000, 2.6293],
[-2.7239, -0.5000, 2.8958],
[-1.8541, -0.5000, 3.0909],
[-0.9386, -0.5000, 3.2100],
[0, -0.5000, 3.2500]])
u = np.arange(1, 17).reshape(4, -1)
b = np.arange(4)
print("utsolve: {}".format(calibrate_magnetometer(a)))
if __name__ == "__main__":
main()
这似乎像 matlab 代码一样工作。剩下的问题主要是“扁平化”D 中的内部 np.arrays 和 select 来自 qr() 的第二个返回值。
import numpy as np
from scipy.linalg import qr, svd
def calibrate_magnetometer(calibration_data: np.ndarray):
"""
performs magnetometer calibration from a set of data
using Merayo technique with a non iterative algoritm
J.Merayo et al. "Scalar calibration of vector magnemoters"
Meas. Sci. Technol. 11 (2000) 120-132.
X : a Nx3 (or 3xN) data matrix
each row (columns) contains x, y, z measurements
N must be such that the data set describes
as completely as possible the 3D space
In any case N > 10
The calibration tries to find the best 3D ellipsoid that fits the data set
and returns the parameters of this ellipsoid
U : shape ellipsoid parameter, (3x3) upper triangular matrix
c : ellipsoid center, (3x1) vector
Ellipsoid equation : (v-c)'*(U'*U)(v-c) = 1
with v a rough triaxes magnetometer measurement
calibrated measurement w = U*(v-c)
author : Alain Barraud, Suzanne Lesecq 2008
"""
(n, m) = calibration_data.shape
if m > 3 and n == 3: # check that x is not transposed
calibration_data = calibration_data.T
n = m
m = 3
if n <= 10:
return False # not enough data for calibration
# write the ellipsoid equation as D*p=0
# the best parameter is the solution of min||D*p|| with ||p||=1
# form D matix from X measurements
x = calibration_data[:, 0]
y = calibration_data[:, 1]
z = calibration_data[:, 2]
D = [np.square(x), np.square(y), np.square(z),
np.multiply(x,y), np.multiply(x,z), np.multiply(y,z),
x, y, z,
np.ones(n)]
D = np.array(D)
D = np.triu(qr(D.T)[1]) # avoids to compute the svd of a large matrix
u, s, v = svd(D) # because usually N may be very large
p = v[-1]
if p[0] < 0:
p = -p
# the following matrix a(p) must be positive definite.
# the optimization done by svd does not include such a constrain
# with "good" data the constrain is allways satisfied
# with too poor data A may fail to be positive definite
# in this case the calibration fails
a = np.array([[p[0], p[3] / 2, p[4] / 2],
[p[3] / 2, p[1], p[5] / 2],
[p[4] / 2, p[5] / 2, p[2]]])
uu = np.linalg.cholesky(a)
b = np.array([p[6], p[7], p[8]])
vv = utsolve(uu, b / 2, m)
d = p[9]
s = 1 / np.sqrt(vv * vv.T - d)
calibrated_c = -usolve(uu, vv, m) # ellipsoid center, or offset error
calibrated_u = s * uu # shape ellipsoid parameter
return calibrated_u, calibrated_c
def utsolve(u, b, n):
# solves u.T*x=b
x = np.zeros(n)
x[0] = b[0] / u[0, 0]
for k in range(1, n):
x[k] = (b[k] - np.matmul(x[0:k], u[0:k, k])) / u[k, k]
return x
def usolve(u, b, n):
# solves u*x=b
x = np.zeros((n, 1))
x[n - 1] = b[n - 1] / u[n - 1, n - 1]
for k in reversed(range(0, n - 1)):
x[k] = (b[k] - np.matmul(u[k, k + 1:n], x[k + 1:n].T[0])) / u[k, k]
return x.T[0]