?

%互補(bǔ)濾波 單軸
complementation_filtered = zeros(m,1);
adaption_complementation_filtered = zeros(m,1);


coeff = 0.75;


for i=2:m
complementation_filtered(i) = (complementation_filtered(i-1) + gx(i)*dt) * coeff + (1 - coeff) * roll_acc(i);
if abs(gx(i)) > 0.2
adaption_complementation_filtered(i) = (adaption_complementation_filtered(i-1) + gx(i)*dt) * coeff + (1 - coeff) * roll_acc(i);
else
        adaption_complementation_filtered(i) = (adaption_complementation_filtered(i-1) + gx(i)*dt) * 0.005 + (1 - 0.005) * roll_acc(i);
end
end


figure(6)
subplot(411)
title('滾轉(zhuǎn)');
plot(t1,complementation_filtered,'r',t1,roll_gyro);grid;
legend('互補(bǔ)濾波','角速度積分')
subplot(412)
plot(t1,complementation_filtered,'r',t1,roll_acc);grid;
legend('互補(bǔ)濾波','加速度參考')
subplot(413)
plot(t1,complementation_filtered,'r',t1,roll);grid;
legend('互補(bǔ)濾波','參考角度')
subplot(414)
plot(t1,adaption_complementation_filtered,'r',t1,roll);grid;
legend('自適應(yīng)互補(bǔ)','參考角度')

?

?

首先我們可以看出互補(bǔ)濾波的結(jié)果不再像陀螺儀角度那樣，隨著時(shí)間漂移。實(shí)際飛行時(shí)，固定的權(quán)重值很難找到理想值，要不陀螺儀的權(quán)重大了，動(dòng)態(tài)性能還可以，大致能跟上角度，但是不能靜態(tài)保持；加速度權(quán)重大了，噪音大，另外動(dòng)態(tài)性能差，原地來回?cái)[動(dòng)時(shí)，得到的角度幅值會(huì)很小，這里的數(shù)據(jù)很難看出這個(gè)問題。 所以最好是當(dāng)飛行器在動(dòng)態(tài)過程時(shí)，我們更相信陀螺儀，反之，飛行器靜止時(shí)，又更相信加速度計(jì)。即參數(shù)進(jìn)行自適應(yīng)調(diào)整。判定角速度數(shù)據(jù)，大于一定閾值，認(rèn)為在運(yùn)動(dòng)，所以加大陀螺儀權(quán)重。 ? 5、卡爾曼濾波 作為狀態(tài)估計(jì)常用的算法，卡爾曼濾波的卡爾曼增益是動(dòng)態(tài)調(diào)整的，所以這一點(diǎn)比固定權(quán)重的線性互補(bǔ)濾波要好，此外要注意的是卡爾曼的效果好壞與所選用的狀態(tài)變量，建立的模型有很大關(guān)系，不可一概而論卡爾曼就一定很好，具體情況具體分析。 因卡爾曼濾波的使用條件是針對(duì)線性模型，且狀態(tài)分布為高斯分布，所以這里建立兩種線性模型，對(duì)比仿真結(jié)果。 模型一： 狀態(tài)量為角度和角速度偏移，這里認(rèn)為角速度偏移為常值，即角度是上一時(shí)刻的角度加（減）角速度偏移的角度，再加上角速度積分的增量。 轉(zhuǎn)換成狀態(tài)空間形式：

代碼：

%單軸kalman  模型1Q_angle = 0.1;Q_bias  = 0.01;R_angle = 10;     Q = [Q_angle 0; 0 Q_bias];pitchKF1 = zeros(m,1);pitchMeasure1 = zeros(m,1);bias = zeros(m,1);P = [0 00 0];C = [1 0]; for i = 2:m    pitchMeasure1(i) = atan2(-ax(i),-az(i));    A = [1 -dt; 0 1];    %Predicted state estimate x = F.x + B.u    pitchKF1(i) = pitchKF1(i-1) + dt * (gy(i) - bias(i-1));    bias(i) = bias(i-1);    %Predicted estimate covariance P = F.P.F' + Q    P = A*P*A' + Q;    %Innovation y = z - H.xy = pitchMeasure1(i) - pitchKF1(i);    %Innovation covariance S = H.P.H' + R    S = C*P*C' + R_angle;    %Optimal kalman gain K = P.H'/S    K = P * C' / S;    %Updated state estimate x=x + K.y    pitchKF1(i) = pitchKF1(i) + K(1)*y;    bias(i) = bias(i) + K(2)*y;    %Updated estimate covariance P = (I - K.H).P    P = ([1 0; 0 1] - [K(1); K(2)] * C) * P;endfigure(7)plot(t1,pitchKF1,t1,pitch,'r');title('單軸卡爾曼模型結(jié)果驗(yàn)證');xlabel('Time(s)');grid;legend('單軸卡爾曼','參考角度')

?

?

放大圖像：

可以看出，此算法基本能得到正確結(jié)果，除了在某些地方跟蹤不好，具體原因后面再講。 模型二： 狀態(tài)量分別是俯仰角、滾轉(zhuǎn)角以及對(duì)應(yīng)的角速度偏移。與上面的模型相比，這里我們將水平方向的兩軸姿態(tài)合并在一起，這樣易于代碼實(shí)現(xiàn)，否則在軟件中需針對(duì)三個(gè)軸進(jìn)行三次調(diào)用。 模型如下：

代碼：

%兩軸kalman  模型2
pitchKF2 = zeros(m,1);
rollKF2 = zeros(m,1);
bp = zeros(m,1);
bq = zeros(m,1);
pitchMeasure2 = zeros(m,1);
rollMeasure2 = zeros(m,1);
P_out = zeros(m,4,4);


Q_pitch = 0.001; %Calculated from data
Q_roll = 0.001; %Calculated from data
Q_bp = 0.1;
Q_bq = 0.1;
R_pitch = 5; %Accel variance, calculated from data
R_roll = 5; %Accel variance, calculated from data


Q = [Q_pitch 0     0    0
0     Q_roll 0    0
0     0     Q_bp 0
0     0     0    Q_bq];


R = [R_pitch 0     0 0
0     R_pitch 0 0
0     0     0 0
0     0     0 0];


P = [0 0 0 0
0 0 0 0
0 0 10000 0
0 0 0 10000];


H = [1 0 0 0
0 1 0 0
0 0 0 0
0 0 0 0];


I = [1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1];


x = [gy(1)
gx(1)
0
0];
for i = 1:m


pitchMeasure2(i) = atan2(-ax(i),-az(i));
rollMeasure2(i) = atan2(ay(i),-az(i));


F = [1 0 -dt 0
0 1 0 -dt
0 0 1  0
0 0 0  1];


B = [dt 0  0 0
0  dt 0 0
0  0  0 0
0  0  0 0];


u = [gy(i)
gx(i)
0
0];


z = [pitchMeasure2(i)
rollMeasure2(i)
0
0];


%Predicted state estimate
x = F*x + B*u;


%Predicted estimate covariance
P = F*P*F' + dt*Q;


%Measurement residual
y = z - H*x;


%Residual covariance
S = H*P*H' + R;


%Optimal Kalman gain
K = P*H'*pinv(S);


%Updated state estimate
x = x + K*y;


%Updated estimate covariance
P = (I - K*H)*P;


pitchKF2(i) = x(1);
rollKF2(i) = x(2);
bp(i) = x(3);
bq(i) = x(4);
P_out(i,1:4,1:4) = P(1:4,1:4);
end




figure(8)
subplot(211)
plot(t1,rollKF2,t1,roll,'r');title('roll 角度對(duì)比');xlabel('Time(s)');grid;
legend('兩軸卡爾曼','參考角度')
subplot(212)
plot(t1,pitchKF2,t1,pitch,'r');title('pitch 角度對(duì)比');xlabel('Time(s)');grid;
legend('兩軸卡爾曼','參考角度')

?

?

目前為止，我們介紹的都是線性模型的姿態(tài)估計(jì)。以卡爾曼為例，這兩種算法基本能滿足使用要求，但存在不足。由于線性模型的關(guān)系，在只有一個(gè)軸向運(yùn)動(dòng)的時(shí)候，效果還不錯(cuò)，所以如果一個(gè)運(yùn)動(dòng)同時(shí)包含了兩個(gè)軸，這時(shí)候得到的結(jié)果就不準(zhǔn)確了，上述的一些跟蹤不好的地方就是因?yàn)榇藭r(shí)yaw有運(yùn)動(dòng)，所以計(jì)算的結(jié)果受到了影響。 所以，實(shí)際飛行器的運(yùn)動(dòng)為非線性模型，我們也要考慮非線性模型的姿態(tài)估計(jì)。 ? 6、mahony算法 實(shí)際在飛控應(yīng)用時(shí)，以上所描述的線性互補(bǔ)很難滿足使用要求，因此要求更佳的算法。常見的有mahony，屬于非線性互補(bǔ)濾波。算法原理：根據(jù)加速度計(jì)和地磁計(jì)的數(shù)據(jù)，轉(zhuǎn)換到地理坐標(biāo)系后，與對(duì)應(yīng)參考的重力向量和地磁向量進(jìn)行求誤差，這個(gè)誤差用來校正陀螺儀的輸出，然后用陀螺儀數(shù)據(jù)進(jìn)行四元數(shù)更新，再轉(zhuǎn)換到歐拉角。 具體步驟如下：

代碼：

?

%mahonyhalfT = 0.5 * dt;Kp = 0.25;Ki = 0.0;exInt = zeros(m,1);               eyInt = zeros(m,1);ezInt = zeros(m,1); q0 = zeros(m,1);q1 = zeros(m,1);q2 = zeros(m,1);q3 = zeros(m,1);q0q0 = zeros(m,1);q0q1 = zeros(m,1);q0q2 = zeros(m,1);q0q3 = zeros(m,1);q1q1 = zeros(m,1);q1q2 = zeros(m,1);q1q3 = zeros(m,1);q2q2 = zeros(m,1);q2q3 = zeros(m,1);q3q3 = zeros(m,1);q0(1) = 1;rollmahony = zeros(m,1);pitchmahony = zeros(m,1);yawmahony = zeros(m,1);az?=?-az;for i=2:m    q0q0(i)=q0(i-1)*q0(i-1);    q0q1(i)=q0(i-1)*q1(i-1);    q0q2(i)=q0(i-1)*q2(i-1);    q0q3(i)=q0(i-1)*q3(i-1);    q1q1(i)=q1(i-1)*q1(i-1);    q1q2(i)=q1(i-1)*q2(i-1);    q1q3(i)=q1(i-1)*q3(i-1);    q2q2(i)=q2(i-1)*q2(i-1);    q2q3(i)=q2(i-1)*q3(i-1);    q3q3(i)=q3(i-1)*q3(i-1);    norm = sqrt(mx(i)*mx(i)+my(i)*my(i)+mz(i)*mz(i));    mx(i)=mx(i)/norm;    my(i)=my(i)/norm;    mz(i)=mz(i)/norm;    hx = 2.0 * (mx(i) * (0.5 - q2q2(i) - q3q3(i)) + my(i) * (q1q2(i) - q0q3(i)) + mz(i) * (q1q3(i) + q0q2(i)));      hy = 2.0 * (mx(i) * (q1q2(i) + q0q3(i)) + my(i) * (0.5 - q1q1(i) - q3q3(i)) + mz(i) * (q2q3(i) - q0q1(i)));      bx = sqrt(hx * hx + hy * hy);      ez_ef = -hy * bx;    mag_ex = 2.0 * (q1q3(i) - q0q2(i)) * ez_ef;    mag_ey = 2.0 * (q2q3(i) + q0q1(i)) * ez_ef;    mag_ez = (1.0 - 2.0 * q1q1(i) - 2.0 * q2q2(i)) * ez_ef;    norm=sqrt(ax(i)*ax(i)+ay(i)*ay(i)+az(i)*az(i));%把加計(jì)的三維向量轉(zhuǎn)成單位向量    ax(i)=ax(i)/norm;    ay(i)=ay(i)/norm;    az(i)=az(i)/norm;    halfex = (ay(i)*(1.0 - 2.0*q1q1(i) - 2.0*q2q2(i)) - az(i)*(2.0*(q2q3(i)+q0q1(i))));                                        halfey = (az(i)*(2.0*(q1q3(i)-q0q2(i))) - ax(i)*(1.0 - 2.0*q1q1(i) - 2.0*q2q2(i)));    halfez = (ax(i)*(2.0*(q2q3(i)+q0q1(i))) - ay(i)*(2.0*(q1q3(i)-q0q2(i))));    ex = halfex;% + mag_ex;    ey = halfey;% + mag_ey;    ez = halfez;% + mag_ez;    exInt(i) = exInt(i) + ex* Ki* dt;                     eyInt(i) = eyInt(i) + ey* Ki* dt;    ezInt(i) = ezInt(i) + ez* Ki* dt;     gx(i) = (gx(i) + Kp*ex + exInt(i))*halfT;                               gy(i) = (gy(i) + Kp*ey + eyInt(i))*halfT;    gz(i) = (gz(i) + Kp*ez + ezInt(i))*halfT;    q0(i) = q0(i-1) + (-q1(i-1)*gx(i)  - q2(i-1)*gy(i) - q3(i-1)*gz(i));%四元數(shù)微分方程    q1(i) = q1(i-1) + ( q0(i-1)*gx(i) + q2(i-1)*gz(i) - q3(i-1)*gy(i));    q2(i) = q2(i-1) + ( q0(i-1)*gy(i)  - q1(i-1)*gz(i) + q3(i-1)*gx(i));    q3(i) = q3(i-1) + ( q0(i-1)*gz(i) + q1(i-1)*gy(i)  - q2(i-1)*gx(i));    norm = sqrt(q0(i)*q0(i) + q1(i)*q1(i) + q2(i)*q2(i) + q3(i)*q3(i));%四元數(shù)規(guī)范化    q0(i) = q0(i) / norm;    q1(i) = q1(i) / norm;    q2(i) = q2(i) / norm;    q3(i) = q3(i) / norm;    rollmahony(i)   = atan2(2 * q2(i) * q3(i) + 2 * q0(i) * q1(i), -2 * q1(i) * q1(i) - 2 *q2(i)* q2(i) + 1); %roll    pitchmahony(i)  = (asin(2 * q0(i) * q2(i) - 2 * q1(i)* q3(i))); % pitch    yawmahony(i)    = atan2(2 * q1(i) * q2(i) + 2 * q0(i) * q3(i) , -2 *q2(i) * q2(i) - 2 * q3(i) * q3(i) +1);endfigure(9)subplot(311)plot(t1,rollmahony,t1,roll,'r');title('roll 角度對(duì)比');xlabel('Time(s)');grid;legend('mahony','參考角度')subplot(312)plot(t1,pitchmahony,t1,pitch,'r');title('pitch 角度對(duì)比');xlabel('Time(s)');grid;legend('mahony','參考角度')subplot(313)plot(t1,yawmahony,t1,yaw,'r');title('yaw 角度對(duì)比');xlabel('Time(s)');grid;legend('mahony','參考角度')

?

?

圖中可以看出，算法結(jié)果基本能跟蹤參考角度，性能較好。實(shí)際工程中，我們也常常使用mahony算法進(jìn)行姿態(tài)估計(jì)。 ? 7、擴(kuò)展卡爾曼濾波 由于是非線性模型，所以在卡爾曼的基礎(chǔ)上，我們進(jìn)行改進(jìn)，采用擴(kuò)展卡爾曼濾波算法。 模型一： PX4飛控早期的attitude_estimator_ekf版本。

此為開源代碼，讀者可自行進(jìn)行移植測(cè)試。 ? 模型二： 參考書目 Nonami K, Kendoul F, Suzuki S, et al.Autonomous Flying Robots: Unmanned Aerial Vehicles and Micro AerialVehicles[M]. Springer Publishing Company, Incorporated, 2010. 第10章

這里筆者進(jìn)行了修改，將7狀態(tài)量縮減成4狀態(tài)量，只更新四元數(shù)。 代碼：

?

p=rollrate;q=pitchrate;r=yawrate;%ACC的測(cè)量噪聲var_ax=5;var_ay=5;var_az=5;%mag的測(cè)量噪聲var_mx=50;var_my=50;var_mz=50;%狀態(tài)變量的過程噪聲P1=[1,1,1,1];P=diag(P1);x_e=[1;0;0;0];Pitch = zeros(m,1);Roll = zeros(m,1);Yaw = zeros(m,1); %狀態(tài)轉(zhuǎn)移矩陣F = eye(4,4);%卡爾曼增益矩陣K = zeros(4,3);%觀測(cè)矩陣H = zeros(3,4);Hdt=0.5*dt;Q1 = 0.00001;%Q矩陣中的數(shù)也是經(jīng)驗(yàn)值得到，需要在考慮q中的參數(shù)Q2 = 0.01;R=zeros(6,6);R(1,1)=var_ax;R(2,2)=var_ay;R(3,3)=var_az;R(4,4)=var_mx;R(5,5)=var_my;R(6,6)=var_mz;% R=zeros(3,3);% R(1,1)=var_ax;% R(2,2)=var_ay;% R(3,3)=var_az;for i=1:m    norm = 1.0/sqrt(x_e(1,1)*x_e(1,1)+x_e(2,1)*x_e(2,1)+x_e(3,1)*x_e(3,1)+x_e(4,1)*x_e(4,1));for n=1:4        x_e(n,1)=x_e(n,1)*norm;    end    %狀態(tài)轉(zhuǎn)移矩陣    F = [              1,  -Hdt*(p(i)),  -Hdt*(q(i)),  -Hdt*(r(i));        Hdt*(p(i)),                1,  Hdt*(r(i)), -Hdt*(q(i));        Hdt*(q(i)), -Hdt*(r(i)),                1,  Hdt*(p(i));        Hdt*(r(i)),  Hdt*(q(i)), -Hdt*(p(i)),                1;];    dTemp(1)=x_e(1,1)-Hdt*(p(i))*x_e(2,1)-Hdt*(q(i))*x_e(3,1)-Hdt*(r(i))*x_e(4,1);    dTemp(2)=x_e(2,1)+Hdt*(p(i))*x_e(1,1)-Hdt*(q(i))*x_e(4,1)+Hdt*(r(i))*x_e(3,1);    dTemp(3)=x_e(3,1)+Hdt*(p(i))*x_e(4,1)+Hdt*(q(i))*x_e(1,1)-Hdt*(r(i))*x_e(2,1);    dTemp(4)=x_e(4,1)-Hdt*(p(i))*x_e(3,1)+Hdt*(q(i))*x_e(2,1)+Hdt*(r(i))*x_e(1,1);for j=1:4        x_e(j,1)=dTemp(j);    end    %協(xié)方差一步預(yù)測(cè)更新    P=F*P*F';    %加入噪音項(xiàng)    P(1,1)=P(1,1)+Q1;    P(2,2)=P(2,2)+Q1;    P(3,3)=P(3,3)+Q1;    P(4,4)=P(4,4)+Q1;    y(1,1)= -2.0*(x_e(2,1)*x_e(4,1)-x_e(1,1)*x_e(3,1));    y(2,1)= -2.0*(x_e(1,1)*x_e(2,1)+x_e(3,1)*x_e(4,1));    y(3,1)= -(x_e(1,1)*x_e(1,1)-x_e(2,1)*x_e(2,1)-x_e(3,1)*x_e(3,1)+x_e(4,1)*x_e(4,1));    %測(cè)量的Acc數(shù)據(jù)    norm=sqrt(ax(i)*ax(i)+ay(i)*ay(i)+az(i)*az(i));%把加計(jì)的三維向量轉(zhuǎn)成單位向量    ax(i)=ax(i)/norm;    ay(i)=ay(i)/norm;    az(i)=az(i)/norm;    y_s(1,1) = -ax(i);    y_s(2,1) = -ay(i);    y_s(3,1) = az(i);    norm = sqrt(mx(i)*mx(i)+my(i)*my(i)+mz(i)*mz(i));    mx(i)=mx(i)/norm;    my(i)=my(i)/norm;    mz(i)=mz(i)/norm;    y_s(4,1) = mx(i);    y_s(5,1) = my(i);    y_s(6,1) = mz(i);    hx = 2.0 * (mx(i) * (0.5 - x_e(3,1)*x_e(3,1) - x_e(4,1)*x_e(4,1)) + my(i) * (x_e(2,1)*x_e(3,1) - x_e(1,1)*x_e(4,1)) + mz(i) * (x_e(2,1)*x_e(4,1) + x_e(1,1)*x_e(3,1)));      hy = 2.0 * (mx(i) * (x_e(2,1)*x_e(3,1) + x_e(1,1)*x_e(4,1)) + my(i) * (0.5 - x_e(2,1)*x_e(2,1) - x_e(4,1)*x_e(4,1)) + mz(i) * (x_e(3,1)*x_e(4,1) - x_e(1,1)*x_e(2,1)));      bx = sqrt(hx * hx + hy * hy);      bz = 2.0 * (mx(i) * (x_e(2,1)*x_e(4,1) - x_e(1,1)*x_e(3,1)) + my(i) * (x_e(3,1)*x_e(4,1) + x_e(1,1)*x_e(2,1)) + mz(i) * (0.5 - x_e(2,1)*x_e(2,1) - x_e(3,1)*x_e(3,1)));    y(4,1)=bx*(x_e(1,1)*x_e(1,1) + x_e(2,1)*x_e(2,1) - x_e(3,1)*x_e(3,1) - x_e(4,1)*x_e(4,1)) + 2*bz*(x_e(2,1)*x_e(4,1) - x_e(1,1)*x_e(3,1));    y(5,1)=2*bx*(x_e(2,1)*x_e(3,1) - x_e(1,1)*x_e(4,1)) + 2*bz*(x_e(3,1)*x_e(4,1) + x_e(1,1)*x_e(2,1));    y(6,1)=2*bx*(x_e(2,1)*x_e(4,1) + x_e(1,1)*x_e(3,1)) + bz*(x_e(1,1)*x_e(1,1) - x_e(2,1)*x_e(2,1) - x_e(3,1)*x_e(3,1) + x_e(4,1)*x_e(4,1));        %觀測(cè)矩陣更新%         H = [ 2.0*x_e(3,1), -2.0*x_e(4,1),  2.0*x_e(1,1), -2.0*x_e(2,1);%             -2.0*x_e(2,1), -2.0*x_e(1,1), -2.0*x_e(4,1), -2.0*x_e(3,1);%             -2.0*x_e(1,1),  2.0*x_e(2,1),  2.0*x_e(3,1), -2.0*x_e(4,1);];         H = [ 2*x_e(3,1), -2*x_e(4,1),  2*x_e(1,1), -2*x_e(2,1);            -2*x_e(2,1), -2*x_e(1,1), -2*x_e(4,1), -2*x_e(3,1);            -2*x_e(1,1),  2*x_e(2,1),  2*x_e(3,1), -2*x_e(4,1);            2*( x_e(1,1)*bx - x_e(3,1)*bz), 2*( x_e(2,1)*bx + x_e(4,1)*bz), 2*(-x_e(3,1)*bx - x_e(1,1)*bz), 2*(-x_e(4,1)*bx + x_e(2,1)*bz);            2*(-x_e(4,1)*bx + x_e(2,1)*bz), 2*( x_e(3,1)*bx + x_e(1,1)*bz), 2*( x_e(2,1)*bx + x_e(4,1)*bz), 2*(-x_e(1,1)*bx + x_e(3,1)*bz);            2*( x_e(3,1)*bx + x_e(1,1)*bz), 2*( x_e(4,1)*bx - x_e(2,1)*bz), 2*( x_e(1,1)*bx - x_e(3,1)*bz), 2*( x_e(2,1)*bx + x_e(4,1)*bz)];         %計(jì)算卡爾曼增益        K=P*H'*pinv(H*P*H'+R);        %狀態(tài)估計(jì)        x_e=x_e+K*(y_s-y);        %x_e=x_e;???? P=P-K*H*P;        norm = 1.0/sqrt(x_e(1,1)*x_e(1,1)+x_e(2,1)*x_e(2,1)+x_e(3,1)*x_e(3,1)+x_e(4,1)*x_e(4,1));        for n=1:4            x_e(n,1)=x_e(n,1)*norm;        end        %四元數(shù)轉(zhuǎn)歐拉角        Pitch(i) = asin(2*(x_e(1,1)*x_e(3,1)- x_e(2,1)*x_e(4,1)));        Roll(i) = atan2(2*(x_e(1,1)*x_e(2,1)+x_e(3,1)*x_e(4,1)),1-2*(x_e(2,1)*x_e(2,1)+x_e(3,1)*x_e(3,1)));        Yaw(i) = atan2(2*(x_e(2,1)*x_e(3,1)+x_e(1,1)*x_e(4,1)),1-2*(x_e(3,1)*x_e(3,1)+x_e(4,1)*x_e(4,1)));end   figure(11)subplot(311)plot(t1,Roll,t1,roll,'r');title('roll 角度對(duì)比');xlabel('Time(s)');grid;legend('EKF','參考角度')subplot(312)plot(t1,Pitch,t1,pitch ,'r');title('pitch 角度對(duì)比');xlabel('Time(s)');grid;legend('EKF','參考角度')subplot(313)plot(t1,Yaw,t1,yaw,'r');title('yaw 角度對(duì)比');xlabel('Time(s)');grid;legend('EKF','參考角度')

?

?

Yaw部分是因?yàn)閰⒖冀嵌热サ袅似?，只有相?duì)角度，所以實(shí)際是正確的跟蹤。上圖表明，EKF能達(dá)到較好的效果，滿足使用要求。 ? 至此，我們已經(jīng)總結(jié)了在姿態(tài)估計(jì)中常用的幾種算法，每個(gè)算法的不足與優(yōu)點(diǎn)也大概介紹了。實(shí)際在應(yīng)用時(shí)，并不一定是理論越復(fù)雜就一定好用，比如EKF，如果模型維數(shù)過多，則計(jì)算量大大增加，且增加了算法發(fā)散的風(fēng)險(xiǎn)，所以工程應(yīng)用，只選用滿足要求的最佳算法。 ?

總結(jié)一下： 1、? 坐標(biāo)系對(duì)應(yīng)是姿態(tài)估計(jì)非常重要的一環(huán)，特別是移植； 2、? 如何調(diào)參？工程上進(jìn)行算法開發(fā)時(shí)，筆者建議的流程是先分析問題，選用相應(yīng)的算法，并進(jìn)行設(shè)計(jì)，取飛行數(shù)據(jù)，然后在matlab仿真環(huán)境下進(jìn)行仿真調(diào)試，最后轉(zhuǎn)換成嵌入式代碼實(shí)現(xiàn)功能。這樣做的好處是，如果在嵌入式環(huán)境下進(jìn)行大量的調(diào)參時(shí)，非常繁瑣，不方便以及不直觀。當(dāng)然，現(xiàn)在有基于模型的開發(fā)方式，這里不做過多敘述。具體如何調(diào)參，可參見視頻教程； 3、? 工程應(yīng)用時(shí)，要考慮震動(dòng)對(duì)數(shù)據(jù)以及算法的影響，動(dòng)加速度對(duì)重力參考的影響，外界地磁干擾對(duì)地磁參考的影響，如何在算法中避免這些情況，或者說出現(xiàn)這些情況下如何保證算法不崩潰，即魯棒性； 4、? 沒有絕對(duì)的最好的算法。

編輯：黃飛