求大神指点，下面的滤波哪里不对。用网上比较多的一个方法KpKi滤波，是收敛的；
但用下面程序对同样数据滤波，陀螺仪的姿态是发散的，陀螺仪融入了加计和磁强计的误差，程序考虑了陀螺仪的零偏gyro_bias'，但是仿真时，零偏一直为0，没起作用
整个程序大概意思是把陀螺仪的数据gyro结合上次最优估计进行一步转移矩阵F得出预估值，然后直接转为四元数q，把加计和磁强计的误差融合进四元数进行滤波。
量测阵H由H1，H2组合: H1/H2由下式推出：
g(导航加速度)在机体坐标系的分量=H1*q（q是预估值转化的四元数）
mag（导航磁强计）在机体坐标系分量=H2*q，
下面是程序：
function [quat,gyro_bias,P] = ImuFusion(q,acc,gyro,mag,Time,gyro_bias,P)
g=9.79973;
Q=diag([0.0001,0.0001,0.0001,0.0001,0.0005,0.0005,0.0005]);
R=diag([0.00035,0.00035,0.0007,0.00004,0.00004,0.00005]);
F=zeros(1,49);
A=zeros(1,49);
B=zeros(1,49);
T=zeros(6,1);
P1=zeros(7,7);
Y = zeros(7:1);
H=zeros(6,7);
halfT=Time/2;
gyro = gyro -gyro_bias';
%%integrate quaternion rate and normalise
q(1) = q(1) + (-q(2)*gyro(1) - q(3)*gyro(2) - q(4)*gyro(3)) * halfT; %预估值
q(2) = q(2) + (q(1)*gyro(1) + q(3)*gyro(3) - q(4)*gyro(2)) * halfT;
q(3) = q(3) + (q(1)*gyro(2) - q(2)*gyro(3) + q(4)*gyro(1)) * halfT;
q(4) = q(4) + (q(1)*gyro(3) + q(2)*gyro(2) - q(3)*gyro(1)) * halfT;
norm = 1/sqrt(sum(q.^2));
q = norm .* q;
%F为Tk-1至Tk时刻的一步转移阵
F=[1,-gyro(1)*halfT,-gyro(2)*halfT,-gyro(3)*halfT,0,0,0;...
gyro(1)*halfT,1, gyro(3)*halfT, -gyro(2)*halfT,0,0,0;...
gyro(2)*halfT,-gyro(3)*halfT,1, gyro(1)*halfT,0,0,0;...
gyro(3)*halfT,gyro(2)*halfT, -gyro(1)*halfT,1,0,0,0;...
0, 0, 0, 0, 1,0,0;...
0, 0, 0, 0, 0,1,0;...
0, 0, 0, 0, 0,0,1;...
]
P1= F* P*F'+ Q; %均方误差
norm = 1/sqrt(sum(mag.^2));
mag= mag * norm*g;
%机体坐标系转换到导航坐标系
hx = 2*mag(1)*(0.5 - q(3)^2 - q(4)^2) + 2*mag(2)*(q(2)*q(3) - q(1)*q(4)) + 2*mag(3)*(q(2)*q(4) + q(1)*q(3));
hy = 2*mag(1)*(q(2)*q(3) + q(1)*q(4)) + 2*mag(2)*(0.5 - q(2)^2 - q(4)^2) + 2*mag(3)*(q(3)*q(4) - q(1)*q(2));
hz = 2*mag(1)*(q(2)*q(4) - q(1)*q(3)) + 2*mag(2)*(q(3)*q(4) + q(1)*q(2)) + 2*mag(3)*(0.5 - q(2)^2 - q(3)^2);
bx = sqrt((hx^2) + (hy^2));
bz = hz;
%导航坐标系转换到机体坐标系
wx = 2*bx*(0.5 - q(3)*q(3) - q(4)*q(4)) + 2*bz*(q(2)*q(4) - q(1)*q(3));
wy = 2*bx*(q(2)*q(3) - q(1)*q(4)) + 2*bz*(q(1)*q(2) + q(3)*q(4));
wz = 2*bx*(q(1)*q(3) + q(2)*q(4)) + 2*bz*(0.5 - q(2)*q(2) - q(3)*q(3));
Ha1 = (-q(3)) * g;
Ha2 = q(4) * g;
Ha3 = -q(1) * g;
Ha4 = q(2) * g;
Hb1 = bx * q(1) - bz * q(3);
Hb2 = bx * q(2) + bz * q(4);
Hb3 = -bx * q(3) - bz * q(1);
Hb4 = -bx * q(4) + bz * q(2);
H = [Ha1,Ha2,Ha3,Ha4,0,0,0; ...
Ha4,-Ha3,Ha2,-Ha1,0,0,0; ...
-Ha3,-Ha4,Ha1,Ha2,0,0,0; ...
Hb1,Hb2,Hb3,Hb4,0,0,0; ...
Hb4,-Hb3,Hb2,-Hb1,0,0,0; ...
-Hb3,-Hb4,Hb1,Hb2,0,0,0; ...
];
K = P1* H' /(H*P1*H'+R) %卡尔曼增益
norm = 1/sqrt(sum(acc.^2));
acc= acc * norm*g;
vx = 2*(q(2)*q(4) - q(1)*q(4))*g;
vy = 2*(q(1)*q(2) + q(3)*q(4))*g;
vz = (q(1)^2-q(2)^2-q(3)^2+q(4)^2)*g;
e1=acc(1)-vx;e2=acc(2)-vy;e3=acc(3)-vz;
e4=mag(1)-wx;e5=mag(2)-wy;e6=mag(3)-wz;
T = [e1;e2;e3;e4;e5;e6];
Y=K*T %
q(1)=q(1)+Y(1); %最优估计
q(2)=q(2)+Y(2);
q(3)=q(3)+Y(3);
q(4)=q(4)+Y(4);
gyro_bias(1)=gyro_bias(1)+Y(5);
gyro_bias(2)=gyro_bias(2)+Y(5);
gyro_bias(3)=gyro_bias(3)+Y(5);
% normalise quaternion
norm = 1/sqrt(sum(q.^2));
q = norm .* q;
quat = q;
P=(eye(7)-K*H)*P1; %更新均方误差
end
|
楼主
标题置顶
标题高亮
