Ahi estudie tu código, entiendo perfectamente lo que haces.
Mi código (el que usan la mayoria de los que haces sistemas de estabilizacion casero) se basa en algunas simplificaciones. básicamente que para angulos pequeños el cos A = 1 y el sin A = A creo y que para pequeñas rotaciones no es importante el orden con que se aplican. Esas simplificaciones hacen que la DCM deje de ser orthonormal. Entonces lo que se hace es renormalizar en cada pasada.
Primero se calcula el error de perpendicularidad entre primera y segunda fila de la DCM y se reparte dicho error a cada fila:
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2,
* error;
y_est = dcmEst(1,
* error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
Luego la tercer fila fila debe ser perpendicular a las otras dos entonces se crea con un producto entre esas filas:
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
finalmente se renormaliza cada fila (se hace que la magnitud de cada vector sea 1)
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
con eso se ahorra cálculo (para no resolver tantos senos y cosenos).
Mi código quedó así:
clear;
imu_interval=0.02;
dcmEst=[1 0 0; 0 1 0; 0 0 1];
gyro=[0 1*0.017453293 0] * imu_interval;
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
graf(18000,2)=zeros;
for n = 1:18000
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2,
* error;
y_est = dcmEst(1,
* error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
graf(n,1)=n*imu_interval;
graf(n,2)=dcmEst(1,1);
end
figure
plot(graf(:,1),graf(:,2));
Ahora si obtengo la variacion esperada en cada elemento de la DCM.
Tambien lo probe en mi IMU con mi uC y lo que obtengo es una deriva bajísima (imaginate que yo estaba metiendo los datos de mi gyro 57 veces más grandes de lo que eran!!!)
Por lo que veo has hecho mucho trabajo en este tema. Nunca leiste los trabajos de william premerlani de DIY Drone? (code.google.com/p/gentlenav/downloads/list) Ellos tienen un sistema de autopiloto para UAV (MatrixPilot) opensource muy pero muy bueno.
Si te parece abrimos un post nuevo en el foro técnico y vamos poniendo ahí los avances con los sensores y la implemetacion de la DCM...
Saludos y gracias por la ayuda!
