bloquer certains paramètres dans fminsearch

Bonjour,

Je cherche a "fitter" une courbe qui peut avoir tournée autour d'un axe ou été translatée ou les deux à la fois.
J'utilise donc fminsearch avec des matrices de changement de repère.
Par contre dans ma fonction de cout j'aimerai dire si je choisis de "fitter" uniquement en utilisant la translation, ou uniquement la rotation ou les 2.
Voici mon code :

Code:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
clear all
close all
clc
 
%% simulation d'une forme simple à retrouver
x=-10:0.1:10;
y=x;
[X,Y]=meshgrid(x,y);
Z=sqrt(X.^2+Y.^2);
XYZtheo=[X(:)';Y(:)';Z(:)'];
 
%% simulation de cette forme simple un peu déviée de la théorie --> à fitter et retrouver les mêmes paramètres
% 1. rotation
aX=2;
aY=10;
aZ=0;
c1=cosd(aX);
c2=cosd(aY);
c3=cosd(aZ);
s1=sind(aX);
s2=sind(aY);
s3=sind(aZ);
R=[c2*c3 -c2*s3 s2;c1*s3+c3*s1*s2 c1*c3-s1*s2*s3 -c2*s1;s1*s3-c1*c3*s2 c3*s1+c1*s2*s3 c1*c2];
 
% 2. translation
tX=0;
tY=4;
tZ=-5;
T=[tX;tY;tZ];
 
 
XYZexp=R*[X(:)';Y(:)';Z(:)']+T;
XYZexp=XYZexp+0.02*(randn(size(XYZexp))-0.5);
XYZexp=XYZexp(:,(end+1)/4:10:3*(end+1)/4); %juste pour prendre moins de pts
Xexp=XYZexp(1,:);
Yexp=XYZexp(2,:);
Zexp=XYZexp(3,:);
 
%% affichage
figure, plot3(X(:),Y(:),Z(:),'.');
hold on
plot3(Xexp,Yexp,Zexp,'.');
hold off
legend('theorique','experimental');
axis equal
grid on
 
%% fit
[aX0 aY0 aZ0 tX0 tY0 tZ0]=deal(1,5,0,0,1,2); % obligé de l'initialiser un peu sinon il ne trouve pas (surtout en tY)
param0=[aX0 aY0 aZ0 tX0 tY0 tZ0];
figure;
options=optimset('PlotFcns',@optimplotfval,'TolFun',1e-12,'TolX',1e-12);
param_opt=fminsearch(@(param) fctCout(param,XYZexp),param0,options); 
[Cost,XYZfit] = fctCout(param_opt,XYZexp);
param_opt % afficher les paramètre optimisés pour les comparer aux vraies valeurs
 
Xfit=XYZfit(1,:);
Yfit=XYZfit(2,:);
Zfit=XYZfit(3,:);
figure, plot3(Xfit,Yfit,sqrt(Xfit.^2+Yfit.^2),'.');
% figure, plot3(X(:),Y(:),Z(:),'.');
hold on
plot3(Xfit,Yfit,Zfit,'.');
hold off
legend('theorique','experimental fitté');
axis equal
grid on
 
 
%% fonction de cout
function [Cost,XYZfit]=fctCout(param,XYZexp)
aX=param(1);
aY=param(2);
aZ=param(3);
tX=param(4);
tY=param(5);
tZ=param(6);
 
c1=cosd(aX);
c2=cosd(aY);
c3=cosd(aZ);
s1=sind(aX);
s2=sind(aY);
s3=sind(aZ);
R=[c2*c3 -c2*s3 s2;c1*s3+c3*s1*s2 c1*c3-s1*s2*s3 -c2*s1;s1*s3-c1*c3*s2 c3*s1+c1*s2*s3 c1*c2];
T=[tX;tY;tZ];
 
%% fit
XYZfit=inv(R)*(XYZexp-T);
Xfit=XYZfit(1,:);
Yfit=XYZfit(2,:);
Zfit=XYZfit(3,:);
Ztheo=sqrt(Xfit.^2+Yfit.^2); %dans le bon repère
 
%% fct de cout à minimiser sur Z dans le même repère
% Cost=std(Zfit-Ztheo); %RMS
% Cost=max(abs(Zfit-Ztheo)); %PV
Cost=std(Zfit-Ztheo)+max(abs(Zfit-Ztheo)); %RMS+PV
 
%% affichage en temps réel
plot3(Xfit,Yfit,Zfit,'.');
hold on
plot3(Xfit,Yfit,Ztheo,'.');
hold off
axis equal
grid on
 
end

Je ne souhaite pas crééer plusieurs fonction de cout par exemple celle du fit uniquement de la rotation en n'utilisant pas les paramètres de translation (qui ont été commentés) :

Code:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
%% fonction de cout
function [Cost,XYZfit]=fctCout_rotation(param,XYZexp)
aX=param(1);
aY=param(2);
aZ=param(3);
%tX=param(4);
%tY=param(5);
%tZ=param(6);
 
c1=cosd(aX);
c2=cosd(aY);
c3=cosd(aZ);
s1=sind(aX);
s2=sind(aY);
s3=sind(aZ);
R=[c2*c3 -c2*s3 s2;c1*s3+c3*s1*s2 c1*c3-s1*s2*s3 -c2*s1;s1*s3-c1*c3*s2 c3*s1+c1*s2*s3 c1*c2];
%T=[tX;tY;tZ];
 
%% fit
%XYZfit=inv(R)*(XYZexp-T);
XYZfit=inv(R)*XYZexp;
Xfit=XYZfit(1,:);
Yfit=XYZfit(2,:);
Zfit=XYZfit(3,:);
Ztheo=sqrt(Xfit.^2+Yfit.^2); %dans le bon repère
 
%% fct de cout à minimiser sur Z dans le même repère
% Cost=std(Zfit-Ztheo); %RMS
% Cost=max(abs(Zfit-Ztheo)); %PV
Cost=std(Zfit-Ztheo)+max(abs(Zfit-Ztheo)); %RMS+PV
 
%% affichage en temps réel
plot3(Xfit,Yfit,Zfit,'.');
hold on
plot3(Xfit,Yfit,Ztheo,'.');
hold off
axis equal
grid on
 
end

Merci ;)

D'autre part, je chercherai aussi une façon de dire à mes paramètres de ne pas depasser une certaine plage (par exemple la rotation en Z, aZ ne doit pas dépasser 2deg). Je n'ai toujours pas trouver comment bloquer ces paramètres.

Merci encore ;)