Symbolic에 의한 Jacobian 연산

물체 추적이나 필터 설계에서 Jacobian을 계산해야 할 경우가 많다.  자세 행렬의 Jacobian을 구할 경우 회전 행렬을 각도 변수에 대해 편미분해야 하며 손으로 계산하기에는 복잡하다.  Matlab의 symbolic연산을 이용하여 Qurternion을 이용하는 회전 행렬의 편미분을 계산해 보자.

% Symbolic 연산 테스트

% Qurternion의 Jacobian 계산

% 01/2014, funMV

clear; clc;

syms alpa; % 변수 alpa를 symbol로 선언 

syms beta;

syms gama;

cos_alpa = cos(alpa/2); sin_alpa=sin(alpa/2);

cos_beta = cos(beta/2); sin_beta=sin(beta/2);

cos_gama = cos(gama/2); sin_gama=sin(gama/2);

q1=cos_alpa*cos_beta*sin_gama-sin_alpa*sin_beta*cos_gama;

q2=cos_alpa*sin_beta*cos_gama+sin_alpa*cos_beta*sin_gama;

q3=sin_alpa*cos_beta*cos_gama-cos_alpa*sin_beta*sin_gama;

q4=cos_alpa*cos_beta*cos_gama+sin_alpa*sin_beta*sin_gama;

%{

q1 =

cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)

%} 

jacobian(q1,alpa)

% -(cos(alpa/2)*cos(gama/2)*sin(beta/2))/2-(cos(beta/2)*sin(alpa/2)*sin(gama/2))/2 

jacobian(q1,beta)

% -(cos(beta/2)*cos(gama/2)*sin(alpa/2))/2-(cos(alpa/2)*sin(beta/2)*sin(gama/2))/2 

jacobian(q1,gama)

% (sin(alpa/2)*sin(beta/2)*sin(gama/2))/2+(cos(alpa/2)*cos(beta/2)*cos(gama/2))/2

jacobian(q2,alpa)

% (cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2

jacobian(q2,beta)

% (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2 - (sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 

jacobian(q2,gama)

% (cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2 

% product of two quaternion b and q

syms b1;

syms b2;

syms b3;

syms b4;

qq1 = b1*q1-[b2 b3 b4]*[q2; q3; q4];

qq2 = (b1*[q2 q3 q4]+q1*[b2 b3 b4])+(cross([b2 b3 b4]',[q2 q3 q4]'))';

%{

qq1 =

  b1*(cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)) 

- b2*(cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)) 

- b3*(cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)) 

- b4*(sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2))

  

qq2(1)= b1*(cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)) 

   + b2*(cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)) 

   + b3*(sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2)) 

   - b4*(cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)), 

qq2(2)= b1*(cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)) 

   - b2*(sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2))

   + b3*(cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)) 

   + b4*(cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)), 

qq2(3)= b1*(sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2))

   + b2*(cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)) 

   - b3*(cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)) 

   + b4*(cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2))

 %}

jacobian(qq1, b1)

%cos(alpa/2)*cos(beta/2)*sin(gama/2) - cos(gama/2)*sin(alpa/2)*sin(beta/2)

jacobian(qq1, b2)

%- cos(alpa/2)*cos(gama/2)*sin(beta/2) - cos(beta/2)*sin(alpa/2)*sin(gama/2)   

jacobian(qq1, b3)

%cos(alpa/2)*sin(beta/2)*sin(gama/2) - cos(beta/2)*cos(gama/2)*sin(alpa/2)

jacobian(qq1, b4)

%- sin(alpa/2)*sin(beta/2)*sin(gama/2) -%cos(alpa/2)*cos(beta/2)*cos(gama/2)

jacobian(qq1, alpa)

%{

b4*((cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2) 

- b2*((cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2) 

- b3*((sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 + (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2) 

- b1*((cos(alpa/2)*cos(gama/2)*sin(beta/2))/2 + (cos(beta/2)*sin(alpa/2)*sin(gama/2))/2)

%}

jacobian(qq2, b1)

% cos(alpa/2)*cos(gama/2)*sin(beta/2) + cos(beta/2)*sin(alpa/2)*sin(gama/2)

% cos(beta/2)*cos(gama/2)*sin(alpa/2) - cos(alpa/2)*sin(beta/2)*sin(gama/2)

% sin(alpa/2)*sin(beta/2)*sin(gama/2) + cos(alpa/2)*cos(beta/2)*cos(gama/2)

jacobian(qq2, alpa)

%{

 b1*((cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2) 

- b2*((cos(alpa/2)*cos(gama/2)*sin(beta/2))/2 + (cos(beta/2)*sin(alpa/2)*sin(gama/2))/2) 

- b3*((cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2) 

- b4*((sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 + (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2),

 b1*((sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 + (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2) 

+ b2*((cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2) 

- b3*((cos(alpa/2)*cos(gama/2)*sin(beta/2))/2 + (cos(beta/2)*sin(alpa/2)*sin(gama/2))/2) 

+ b4*((cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2),

 b2*((sin(alpa/2)*sin(beta/2)*sin(gama/2))/2 + (cos(alpa/2)*cos(beta/2)*cos(gama/2))/2) 

- b1*((cos(beta/2)*cos(gama/2)*sin(alpa/2))/2 - (cos(alpa/2)*sin(beta/2)*sin(gama/2))/2) 

- b3*((cos(alpa/2)*cos(beta/2)*sin(gama/2))/2 - (cos(gama/2)*sin(alpa/2)*sin(beta/2))/2) 

- b4*((cos(alpa/2)*cos(gama/2)*sin(beta/2))/2 + (cos(beta/2)*sin(alpa/2)*sin(gama/2))/2)

%}