100 lines
4.1 KiB
Matlab
100 lines
4.1 KiB
Matlab
|
function state = algo_init(obj, st, vRef)
|
||
|
|
algo_info = load("algo_info.m");
|
||
|
|
algo = algo_info.algo;
|
||
|
|
d = algo_info.d;
|
||
|
|
ang = algo_info.ang;
|
||
|
|
save_state = algo_info.save_state;
|
||
|
|
fDeggreCosine = @(a,b,c) acosd(((a^2)+(b^2)-(c^2)) ...
|
||
|
|
/(2*a*b));
|
||
|
|
switch(algo);
|
||
|
|
case 0;
|
||
|
|
|
||
|
|
d(1,1) = norm((obj.K*st(1:obj.nodeStateLength))(1:3));
|
||
|
|
d(2,1) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
|
||
|
|
algo = 1;
|
||
|
|
state = st;
|
||
|
|
al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
|
||
|
|
|
||
|
|
case 1;
|
||
|
|
set_point = [2 0 0 0 0 0]';
|
||
|
|
state = obj.Av2(st,[1 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
||
|
|
al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
|
||
|
|
if norm(al_state - set_point) < .1
|
||
|
|
d(1,2) = norm((obj.K*st(1:obj.nodeStateLength))(1:3)); ... from sensor
|
||
|
|
d(2,2) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
|
||
|
|
ang(1) = 180 - fDeggreCosine(al_state(1),d(1,2),d(1,1));
|
||
|
|
ang(2) = 180 - fDeggreCosine(al_state(1),d(2,2),d(2,1));
|
||
|
|
save_state = al_state;
|
||
|
|
printf("<algo_init>Estimate stage 1 \n");
|
||
|
|
cal_al_state = [d(1,2)*cosd(ang(1))+al_state(1);
|
||
|
|
d(1,2)*sind(ang(1))+al_state(2);
|
||
|
|
0 ;
|
||
|
|
d(2,2)*cosd(ang(2))+al_state(1);
|
||
|
|
d(2,2)*sind(ang(2))+al_state(2);
|
||
|
|
0]
|
||
|
|
algo=2;
|
||
|
|
else
|
||
|
|
al_state = obj.param.model.A * al_state ...
|
||
|
|
+ obj.param.model.B ...
|
||
|
|
* arrayfun(obj.param.model.limit_motor, ...
|
||
|
|
(obj.param.model.Nst * set_point ...
|
||
|
|
- obj.param.model.Kst*al_state ));
|
||
|
|
state(1:3) = al_state(1:3);
|
||
|
|
state(obj.nodeStateLength+1:obj.nodeStateLength+3) = al_state(4:6);
|
||
|
|
endif
|
||
|
|
case 2;
|
||
|
|
set_point = [1 1 0 0 0 0]';
|
||
|
|
state = obj.Av2(st,[1 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
||
|
|
al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
|
||
|
|
if norm(al_state - set_point) < .1
|
||
|
|
printf("<algo_init>Estimate stage 2 \n");
|
||
|
|
cal_al_state = [al_state(1:3);
|
||
|
|
d(1,2)*cosd(ang(1))+save_state(1);
|
||
|
|
d(1,2)*sind(ang(1))+save_state(2);
|
||
|
|
0 ;
|
||
|
|
d(2,2)*cosd(ang(2))+save_state(1);
|
||
|
|
d(2,2)*sind(ang(2))+save_state(2);
|
||
|
|
0];
|
||
|
|
d(1,3) = norm((obj.K*st(1:obj.nodeStateLength))(1:3)); ... from sensor
|
||
|
|
d(2,3) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
|
||
|
|
_d(1) = norm((obj.K*cal_al_state(1:obj.nodeStateLength))(1:3));
|
||
|
|
_d(2) = norm((obj.K*cal_al_state(1:obj.nodeStateLength))(4:6));
|
||
|
|
printf("<algo_init> validating...\n");
|
||
|
|
for i = 1:length(ang)
|
||
|
|
if sum( (abs(_d(i) - d(i,3))) < .1 )
|
||
|
|
printf("<algo_init> %i pass\n",i);
|
||
|
|
else
|
||
|
|
printf("<algo_init> inverte on %i\n",i);
|
||
|
|
ang(i) = 180 + fDeggreCosine(save_state(1),d(i,2),d(i,1));
|
||
|
|
endif
|
||
|
|
endfor
|
||
|
|
cal_al_state = [al_state(1:3);
|
||
|
|
d(1,2)*cosd(ang(1))+save_state(1);
|
||
|
|
d(1,2)*sind(ang(1))+save_state(2);
|
||
|
|
0 ;
|
||
|
|
d(2,2)*cosd(ang(2))+save_state(1);
|
||
|
|
d(2,2)*sind(ang(2))+save_state(2);
|
||
|
|
0]
|
||
|
|
if norm(cal_al_state - st(1:9)) < .1
|
||
|
|
printf("<algo_init> done\n");
|
||
|
|
algo=3;
|
||
|
|
else
|
||
|
|
printf("<algo_init> fail\n");
|
||
|
|
algo=9;
|
||
|
|
endif
|
||
|
|
else
|
||
|
|
al_state = obj.param.model.A * al_state ...
|
||
|
|
+ obj.param.model.B ...
|
||
|
|
* arrayfun(obj.param.model.limit_motor, ...
|
||
|
|
(obj.param.model.Nst * set_point ...
|
||
|
|
- obj.param.model.Kst*al_state ));
|
||
|
|
state(1:3) = al_state(1:3);
|
||
|
|
state(obj.nodeStateLength+1:obj.nodeStateLength+3) = al_state(4:6);
|
||
|
|
endif
|
||
|
|
case 3;
|
||
|
|
state = obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
||
|
|
case 9;
|
||
|
|
state = obj.Av2(st,[0 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;;
|
||
|
|
endswitch;
|
||
|
|
save "algo_info.m" ang save_state algo d;
|
||
|
|
endfunction
|