電子發燒友App
? 姿態估計的作用？ 姿態估計是飛控算法的一個基礎部分，而且十分重要。為了完成飛行器平穩的姿態控制，首先需要精準的姿態數據作為控制器的反饋。 ?
飛控姿態估計的難點？ 姿態估計的一個難點主要是一般選用的慣性傳感器，都是MEMS器件，精度相對較差；此外，實際工作中很難準確的判定姿態估計的是否準確。 ? 姿態估計的指標？ 一般考慮三個性能，收斂性、精確性、準確性。
收斂性：即估計出的姿態角數據不會輕易發散，在動態變化時，能很快的收斂到對應的角度；
精確性：比如飛行器放置不動，此時得到的姿態角在0度左右波動，這個波動范圍即考慮的精確性；
準確性：這個比較難以考究，即沒辦法確定所得到的角度的精確程度。一般用外部參考的方法測量，比如飛機上同時掛載自己的飛控以及高精度的IMU設備（比如xsens、sbg等），飛行完成后，對比自己飛控所解算的角度和外部設備的誤差；又或者，在室內裝vicon設備來給出外部參考。注意，驗證算法的時候，最好還是用實際飛行的數據，否則加速度噪聲對算法的影響無法驗證。
? 需要什么基礎？即在進行姿態估計前需要做什么？ 主要對傳感器數據進行校準、濾波。本文主要從工程角度去實現姿態估計算法。 ? 姿態估計如何做？ 根據陀螺儀的角度數據高頻特性好，而加速度計和地磁計得到的角度數據低頻特性好，從而進行互補，得到最優角度。無論什么算法，本質都是陀螺儀積分得到角度，然后根據加速度計和地磁計修正積分的漂移誤差。 ? 工程上每個算法的引入都是為了解決問題的，所以下面將從簡單的互補開始，逐步解釋每個算法的優缺點。 1、? 記錄實際飛行數據，需要保存陀螺儀、加速度、地磁計數據； 如圖所示：
筆者這里采用了現成的模塊，所以有參考的角度數據。
2、對好數據坐標系 即調整數據的正負號，初學者往往會忽略這一點，這個在姿態估計算法中非常重要，很多人經常有疑問，為什么自己移植別人的代碼，一模一樣，但是自己就得不到正確的結果，往往原因都在這里。另外，有時候通過調參會掩蓋這類問題，從而導致算法性能不能達到最優。這里筆者以前右下（xyz）為機體系，另外如果飛行數據噪音很大，可以在仿真中做濾波處理。 ? 3、分析單獨的陀螺儀積分角度與加速度計計算得到的角度數據 方法： 角速度數據直接積分； 加速度數據： 
代碼：
%對比角速度積分與加速度得到的姿態角之間的區別 %角速度積分得到的角度會隨時間漂移 %加速度得到的角度會噪聲很大 roll_gyro = zeros(m,1); roll_acc = zeros(m,1); for i = 2:m if i==1 roll_gyro(1) = gx(1) * dt; else roll_gyro(i) = roll_gyro(i-1) + gx(i) * dt; end roll_acc(i) = atan2(ay(i),-az(i)); end figure(4) plot(t1,roll_gyro,t1,roll_acc,'r');title('roll 角速度、加速度推角度對比');xlabel('Time(s)');grid; pitch_gyro = zeros(m,1); pitch_acc = zeros(m,1); for i = 2:m if i==1 pitch_gyro(1) = gy(1) * dt; else pitch_gyro(i) = pitch_gyro(i-1) + gy(i) * dt; end pitch_acc(i) = atan2(-ax(i),-az(i)); end figure(5) plot(t1,pitch_gyro,t1,pitch_acc,'r');title('pitch 角速度、加速度推角度對比');xlabel('Time(s)');grid;仿真結果：
?
?
如上圖所示，藍色的陀螺積分角度隨著時間會有漂移，而加速度計得到的角度則噪聲很大，都無法使用（這里的數據不是飛行數據，所以噪聲很?。?。所以，既然單獨的角度都各自有缺陷，而恰好一個有漂移一個沒有漂移，一個噪音大一個噪音小，很自然能聯想到用互補的辦法，每個周期的最優角度為兩者加權得到。 ? 4、線性互補濾波 通過設置一個權重值，讓每個周期得到的角度由兩個數據源共同作用，還可以通過調節權重值，選擇是更相信陀螺儀還是加速度。 代碼：
?
%互補濾波 單軸 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('滾轉'); plot(t1,complementation_filtered,'r',t1,roll_gyro);grid; legend('互補濾波','角速度積分') subplot(412) plot(t1,complementation_filtered,'r',t1,roll_acc);grid; legend('互補濾波','加速度參考') subplot(413) plot(t1,complementation_filtered,'r',t1,roll);grid; legend('互補濾波','參考角度') subplot(414) plot(t1,adaption_complementation_filtered,'r',t1,roll);grid; legend('自適應互補','參考角度')仿真結果：
?
?
首先我們可以看出互補濾波的結果不再像陀螺儀角度那樣，隨著時間漂移。實際飛行時，固定的權重值很難找到理想值，要不陀螺儀的權重大了，動態性能還可以，大致能跟上角度，但是不能靜態保持；加速度權重大了，噪音大，另外動態性能差，原地來回擺動時，得到的角度幅值會很小，這里的數據很難看出這個問題。 所以最好是當飛行器在動態過程時，我們更相信陀螺儀，反之，飛行器靜止時，又更相信加速度計。即參數進行自適應調整。判定角速度數據，大于一定閾值，認為在運動，所以加大陀螺儀權重。 ? 5、卡爾曼濾波 作為狀態估計常用的算法，卡爾曼濾波的卡爾曼增益是動態調整的，所以這一點比固定權重的線性互補濾波要好，此外要注意的是卡爾曼的效果好壞與所選用的狀態變量，建立的模型有很大關系，不可一概而論卡爾曼就一定很好，具體情況具體分析。 因卡爾曼濾波的使用條件是針對線性模型，且狀態分布為高斯分布，所以這里建立兩種線性模型，對比仿真結果。 模型一： 狀態量為角度和角速度偏移，這里認為角速度偏移為常值，即角度是上一時刻的角度加（減）角速度偏移的角度，再加上角速度積分的增量。 
轉換成狀態空間形式：
代碼：
%單軸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('單軸卡爾曼模型結果驗證');xlabel('Time(s)');grid;legend('單軸卡爾曼','參考角度')仿真結果：
?
?
放大圖像：
可以看出，此算法基本能得到正確結果，除了在某些地方跟蹤不好，具體原因后面再講。 模型二： 狀態量分別是俯仰角、滾轉角以及對應的角速度偏移。與上面的模型相比，這里我們將水平方向的兩軸姿態合并在一起，這樣易于代碼實現，否則在軟件中需針對三個軸進行三次調用。 
模型如下：
代碼：
%兩軸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 角度對比');xlabel('Time(s)');grid;
legend('兩軸卡爾曼','參考角度')
subplot(212)
plot(t1,pitchKF2,t1,pitch,'r');title('pitch 角度對比');xlabel('Time(s)');grid;
legend('兩軸卡爾曼','參考角度')
仿真結果：
?
?
目前為止，我們介紹的都是線性模型的姿態估計。以卡爾曼為例，這兩種算法基本能滿足使用要求，但存在不足。由于線性模型的關系，在只有一個軸向運動的時候，效果還不錯，所以如果一個運動同時包含了兩個軸，這時候得到的結果就不準確了，上述的一些跟蹤不好的地方就是因為此時yaw有運動，所以計算的結果受到了影響。 所以，實際飛行器的運動為非線性模型，我們也要考慮非線性模型的姿態估計。 ? 6、mahony算法 實際在飛控應用時，以上所描述的線性互補很難滿足使用要求，因此要求更佳的算法。常見的有mahony，屬于非線性互補濾波。算法原理：根據加速度計和地磁計的數據，轉換到地理坐標系后，與對應參考的重力向量和地磁向量進行求誤差，這個誤差用來校正陀螺儀的輸出，然后用陀螺儀數據進行四元數更新，再轉換到歐拉角。 具體步驟如下：
代碼：
?
%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));%把加計的三維向量轉成單位向量 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));%四元數微分方程 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));%四元數規范化 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 角度對比');xlabel('Time(s)');grid;legend('mahony','參考角度')subplot(312)plot(t1,pitchmahony,t1,pitch,'r');title('pitch 角度對比');xlabel('Time(s)');grid;legend('mahony','參考角度')subplot(313)plot(t1,yawmahony,t1,yaw,'r');title('yaw 角度對比');xlabel('Time(s)');grid;legend('mahony','參考角度')
? 仿真結果：
?
?
圖中可以看出，算法結果基本能跟蹤參考角度，性能較好。實際工程中，我們也常常使用mahony算法進行姿態估計。 ? 7、擴展卡爾曼濾波 由于是非線性模型，所以在卡爾曼的基礎上，我們進行改進，采用擴展卡爾曼濾波算法。 模型一： PX4飛控早期的attitude_estimator_ekf版本。
此為開源代碼，讀者可自行進行移植測試。 ? 模型二： 參考書目 Nonami K, Kendoul F, Suzuki S, et al.Autonomous Flying Robots: Unmanned Aerial Vehicles and Micro AerialVehicles[M]. Springer Publishing Company, Incorporated, 2010. 第10章
這里筆者進行了修改，將7狀態量縮減成4狀態量，只更新四元數。 代碼：
?
p=rollrate;q=pitchrate;r=yawrate;%ACC的測量噪聲var_ax=5;var_ay=5;var_az=5;%mag的測量噪聲var_mx=50;var_my=50;var_mz=50;%狀態變量的過程噪聲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); %狀態轉移矩陣F = eye(4,4);%卡爾曼增益矩陣K = zeros(4,3);%觀測矩陣H = zeros(3,4);Hdt=0.5*dt;Q1 = 0.00001;%Q矩陣中的數也是經驗值得到，需要在考慮q中的參數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 %狀態轉移矩陣 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 %協方差一步預測更新 P=F*P*F'; %加入噪音項 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)); %測量的Acc數據 norm=sqrt(ax(i)*ax(i)+ay(i)*ay(i)+az(i)*az(i));%把加計的三維向量轉成單位向量 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)); %觀測矩陣更新% 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)]; %計算卡爾曼增益 K=P*H'*pinv(H*P*H'+R); %狀態估計 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 %四元數轉歐拉角 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 角度對比');xlabel('Time(s)');grid;legend('EKF','參考角度')subplot(312)plot(t1,Pitch,t1,pitch ,'r');title('pitch 角度對比');xlabel('Time(s)');grid;legend('EKF','參考角度')subplot(313)plot(t1,Yaw,t1,yaw,'r');title('yaw 角度對比');xlabel('Time(s)');grid;legend('EKF','參考角度')
仿真結果：
?
?
Yaw部分是因為參考角度去掉了偏移，只有相對角度，所以實際是正確的跟蹤。上圖表明，EKF能達到較好的效果，滿足使用要求。 ? 至此，我們已經總結了在姿態估計中常用的幾種算法，每個算法的不足與優點也大概介紹了。實際在應用時，并不一定是理論越復雜就一定好用，比如EKF，如果模型維數過多，則計算量大大增加，且增加了算法發散的風險，所以工程應用，只選用滿足要求的最佳算法。 ?
總結一下： 1、? 坐標系對應是姿態估計非常重要的一環，特別是移植； 2、? 如何調參？工程上進行算法開發時，筆者建議的流程是先分析問題，選用相應的算法，并進行設計，取飛行數據，然后在matlab仿真環境下進行仿真調試，最后轉換成嵌入式代碼實現功能。這樣做的好處是，如果在嵌入式環境下進行大量的調參時，非常繁瑣，不方便以及不直觀。當然，現在有基于模型的開發方式，這里不做過多敘述。具體如何調參，可參見視頻教程； 3、? 工程應用時，要考慮震動對數據以及算法的影響，動加速度對重力參考的影響，外界地磁干擾對地磁參考的影響，如何在算法中避免這些情況，或者說出現這些情況下如何保證算法不崩潰，即魯棒性； 4、? 沒有絕對的最好的算法。
編輯：黃飛
工商網監
評論