|
本帖最后由 s15200380596 于 2014-4-20 21:38 编辑
程序目的是输入三轴角速度,输出三轴角度。捷联阵和欧拉角转换参考秦永元老师的惯性导航。
但输出的角度与实际各轴角度有很大差别(各轴实际角度已知,LZ想用matlab实现得到角度值)
不知道各位看了有什么意见,谢谢指点。
N=10;%数据个数。
t=0.02;%时间步长
%陀螺测量值
r=[-22.0337 -20.1416 -17.6392 -15.2588 -13.4888 - 12.6953 -12.5732 -11.7798 -10.1318 -7.6294 ]; %偏航角速率 wz
q=[-21.4233 -22.1558 -21.4844 -19.043 -16.1743 -14.0381 -12.3291 -10.376 -7.9346 -4.2114 ]; %横滚角速率 wy
p=[ 18.6157 16.9678 14.8926 13.0005 11.5967 10.1318 9.1553 8.9722 9.5825 11.4136]; %俯仰角速率 wx
%初始化
Phi=zeros(1,N);
Theta=zeros(1,N);
Psi=zeros(1,N);
E=zeros(4,N);
%四元数初值
E(1,1)=1;
E(2,1)=0;
E(3,1)=0;
E(4,1)=0;
%更新
for k=2:N
%龙哥库塔法四元数更新
% W矩阵为[ 0 -p -q -r
% p 0 r -q
% q -r 0 p
% r q -p 0]
omega=1/2*[0,-p(k-1),-q(k-1),-r(k-1);p(k-1),0,r(k-1),-q(k-1);q(k-1),-r(k-1),0,p(k-1);r(k-1),q(k-1),-p(k-1),0];
k1=omega*E(:,k-1);%k1=1/2*W*Q
omega=1/4*[0,-p(k)-p(k-1),-q(k)-q(k-1),-r(k)-r(k-1);
p(k)+p(k-1),0,r(k)+r(k-1),-q(k)-q(k-1);
q(k)+q(k-1),-r(k)-r(k-1),0,p(k)+p(k-1);
r(k)+r(k-1),q(k)+q(k-1),-p(k)-p(k-1),0]; %取t时刻和t+T时刻的角速度平均值当做t+T/2时刻的角速度
k2=omega*(E(:,k-1)+k1*t/2);%k2=1/2(q(t)+k1*t/2)*W(t+T/2)
k3=omega*(E(:,k-1)+k2*t/2);
omega=1/2*[0,-p(k),-q(k),-r(k);p(k),0,r(k),-q(k);q(k),-r(k),0,p(k);r(k),q(k),-p(k),0];
k4=omega*(E(:,k-1)+k3*t);
E(:,k)=E(:,k-1)+t/6*(k1+2*k2+2*k3+k4);
%归一化
E(:,k)=E(:,k)/sqrt(E(:,k)'*E(:,k));
%捷联阵更新
%捷联矩阵为[ q0^2+q1^2-q2^2-q3^2 2(q1*q2-q0*q3) 2(q1*q2+q0*q2)
% 2(q1*q2+q0*q3) q0^2-q1^2+q2^2-q3^2 2(q2*q3-q0*q1)
% 2(q1*q3-q0*q2) 2(q2*q3+q0*q1) q0^2-q1^2-q2^2+q3^2]
T=[E(1,k)^2+E(2,k)^2-E(3,k)^2-E(4,k)^2,2*E(2,k)*E(3,k)-2*E(1,k)*E(4,k),2*E(2,k)*E(4,k)+2*E(1,k)*E(3,k);
2*E(2,k)*E(3,k)+2*E(4,k)*E(1,k),E(1,k)^2-E(2,k)^2+E(3,k)^2-E(4,k)^2,2*E(3,k)*E(4,k)-2*E(1,k)*E(2,k);
2*E(2,k)*E(4,k)-2*E(1,k)*E(3,k),2*E(3,k)*E(4,k)+2*E(1,k)*E(2,k),E(1,k)^2-E(2,k)^2-E(3,k)^2+E(4,k)^2];
%航向姿态更新
Phi(k)=((atan(T(1,2)/T(2,2))))*180/pi;
Theta(k)=((asin ((T(3,2)))))*180/pi;
Psi(k)=((atan(-T(3,1)/T(3,3))))*180/pi;
end
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
你熬了10碗粥,别人一桶水倒进去,淘走90碗,剩下10碗给你,你看似没亏,其实你那10碗已经没有之前的裹腹了,人家的一桶水换90碗,继续卖。说白了,通货膨胀就是,你的钱是挣来的,他的钱是印来的,掺和在一起,你的钱就贬值了。
|