calib.m

来自「足球机器人自动程序」· M 代码 · 共 32 行

clear;

x = 1;
y = 2;

w = [
-5.9 33.3
10.1 33.3
-6.1 18.2
10.3 18.3
];

s = [
58 70
208 60
75 211
266 180
] / 2;

a = zeros(8, 8);
for i = 1:4
	a(i,:) = [ s(i,x), s(i,y), 1, 0, 0, 0, -s(i,x)*w(i,x), -s(i,y)*w(i,x) ];
end;

for i = 1:4
	a(i+4,:) = [ 0, 0, 0, s(i,x), s(i,y), 1, -s(i,x)*w(i,y), -s(i,y)*w(i,y) ];
end;

b = [ w(:,x); w(:,y) ];

m = a \ b