• 飞控姿态解算算法解析

    姿态估计的作用？

    姿态估计是飞控算法的一个基础部分，而且十分重要。为了完成飞行器平稳的姿态控制，首先需要精准的姿态数据作为控制器的反馈。

    飞控姿态估计的难点？

    姿态估计的一个难点主要是一般选用的惯性传感器，都是MEMS器件，精度相对较差；此外，实际工作中很难准确的判定姿态估计的是否准确。

    姿态估计的指标？

    一般考虑三个性能，收敛性、精确性、准确性。

    Ø 收敛性：即估计出的姿态角数据不会轻易发散，在动态变化时，能很快的收敛到对应的角度；

    Ø 精确性：比如飞行器放置不动，此时得到的姿态角在0度左右波动，这个波动范围即考虑的精确性；

    Ø 准确性：这个比较难以考究，即没办法确定所得到的角度的精确程度。一般用外部参考的方法测量，比如飞机上同时挂载自己的飞控以及高精度的IMU设备（比如xsens、sbg等），飞行完成后，对比自己飞控所解算的角度和外部设备的误差；又或者，在室内装vicon设备来给出外部参考。注意，验证算法的时候，最好还是用实际飞行的数据，否则加速度噪声对算法的影响无法验证。

    需要什么基础？即在进行姿态估计前需要做什么？

    主要对传感器数据进行校准、滤波，陀螺仪、加速度、地磁的数据预处理以及滤波算法请看专栏或公众号的其他文章。

    本文主要从工程角度去实现姿态估计算法，相应的理论知识也可见其他文章。

    姿态估计如何做？

    一言以蔽之，根据陀螺仪的角度数据高频特性好，而加速度计和地磁计得到的角度数据低频特性好，从而进行互补，得到最优角度。无论什么算法，本质都是陀螺仪积分得到角度，然后根据加速度计和地磁计修正积分的漂移误差。

    工程上每个算法的引入都是为了解决问题的，所以下面将从简单的互补开始，逐步解释每个算法的优缺点。

    1、 记录实际飞行数据，需要保存陀螺仪、加速度、地磁计数据；

    如图所示：

    笔者这里采用了现成的模块，所以有参考的角度数据。

    2、对好数据坐标系

    即调整数据的正负号，初学者往往会忽略这一点，这个在姿态估计算法中非常重要，很多人经常有疑问，为什么自己移植别人的代码，一模一样，但是自己就得不到正确的结果，往往原因都在这里。

    这里笔者以前右下（xyz）为机体系，另外如果飞行数据噪音很大，可以在仿真中做滤波处理。

    3、分析单独的陀螺仪积分角度与加速度计计算得到的角度数据

    方法：

    角速度数据直接积分；

    加速度数据：

    代码：

    1. %对比角速度积分与加速度得到的姿态角之间的区别
    2. %角速度积分得到的角度会随时间漂移
    3. %加速度得到的角度会噪声很大
    4.  
    5. roll_gyro = zeros(m,1);
    6. roll_acc = zeros(m,1);
    7.  
    8. for i = 2:m
    9.     
    10.     if i==1
    11.         roll_gyro(1) = gx(1) * dt;
    12.     else
    13.         roll_gyro(i) = roll_gyro(i-1) + gx(i) * dt;
    14.     end
    15.     
    16.     roll_acc(i) = atan2(ay(i),-az(i));
    17.     
    18. end
    19.  
    20. figure(4)
    21. plot(t1,roll_gyro,t1,roll_acc,'r');title('roll 角速度、加速度推角度对比');xlabel('Time(s)');grid;
    22.  
    23. pitch_gyro = zeros(m,1);
    24. pitch_acc = zeros(m,1);
    25.  
    26. for i = 2:m
    27.     
    28.     if i==1
    29.         pitch_gyro(1) = gy(1) * dt;
    30.     else
    31.         pitch_gyro(i) = pitch_gyro(i-1) + gy(i) * dt;
    32.     end
    33.     
    34.     pitch_acc(i) = atan2(-ax(i),-az(i));
    35.     
    36. end
    37.  
    38. figure(5)
    39. plot(t1,pitch_gyro,t1,pitch_acc,'r');title('pitch 角速度、加速度推角度对比');xlabel('Time(s)');grid;

    仿真结果：

    如上图所示，蓝色的陀螺积分角度随着时间会有漂移，而加速度计得到的角度则噪声很大，都无法使用（这里的数据不是飞行数据，所以噪声很小）。

    所以，既然单独的角度都各自有缺陷，而恰好一个有漂移一个没有漂移，一个噪音大一个噪音小，很自然能联想到用互补的办法，每个周期的最优角度为两者加权得到。

    4、线性互补滤波

    通过设置一个权重值，让每个周期得到的角度由两个数据源共同作用，还可以通过调节权重值，选择是更相信陀螺仪还是加速度。

    代码：

    1. %互补滤波 单轴
    2. complementation_filtered = zeros(m,1);
    3. adaption_complementation_filtered = zeros(m,1);
    4.  
    5. coeff = 0.75;
    6.  
    7. for i=2:m
    8.     complementation_filtered(i) = (complementation_filtered(i-1) + gx(i)*dt) * coeff + (1 - coeff) * roll_acc(i);
    9.     if abs(gx(i)) > 0.2
    10.         adaption_complementation_filtered(i) = (adaption_complementation_filtered(i-1) + gx(i)*dt) * coeff + (1 - coeff) * roll_acc(i);
    11.     else
    12.         adaption_complementation_filtered(i) = (adaption_complementation_filtered(i-1) + gx(i)*dt) * 0.005 + (1 - 0.005) * roll_acc(i);
    13.     end
    14. end
    15.  
    16. figure(6)
    17. subplot(411)
    18. title('滚转');
    19. plot(t1,complementation_filtered,'r',t1,roll_gyro);grid;
    20. legend('互补滤波','角速度积分')
    21. subplot(412)
    22. plot(t1,complementation_filtered,'r',t1,roll_acc);grid;
    23. legend('互补滤波','加速度参考')
    24. subplot(413)
    25. plot(t1,complementation_filtered,'r',t1,roll);grid;
    26. legend('互补滤波','参考角度')
    27. subplot(414)
    28. plot(t1,adaption_complementation_filtered,'r',t1,roll);grid;
    29. legend('自适应互补','参考角度')

    仿真结果：

    首先我们可以看出互补滤波的结果不再像陀螺仪角度那样，随着时间漂移。实际飞行时，固定的权重值很难找到理想值，要不陀螺仪的权重大了，动态性能还可以，大致能跟上角度，但是不能静态保持；加速度权重大了，噪音大，另外动态性能差，原地来回摆动时，得到的角度幅值会很小，这里的数据很难看出这个问题。

    所以最好是当飞行器在动态过程时，我们更相信陀螺仪，反之，飞行器静止时，又更相信加速度计。即参数进行自适应调整。

    代码：

    判定角速度数据，大于一定阈值，认为在运动，所以加大陀螺仪权重。

    5、卡尔曼滤波

    作为状态估计常用的算法，卡尔曼滤波的卡尔曼增益是动态调整的，所以这一点比固定权重的线性互补滤波要好，此外要注意的是卡尔曼的效果好坏与所选用的状态变量，建立的模型有很大关系，不可一概而论卡尔曼就一定很好，具体情况具体分析。

    因卡尔曼滤波的使用条件是针对线性模型，且状态分布为高斯分布，所以这里建立两种线性模型，对比仿真结果。

    模型一：

    状态量为角度和角速度偏移，这里认为角速度偏移为常值，即角度是上一时刻的角度加（减）角速度偏移的角度，再加上角速度积分的增量。

    转换成状态空间形式：

    代码：

    1. %单轴kalman  模型1
    2. Q_angle = 0.1;
    3. Q_bias  = 0.01;
    4. R_angle = 10;     
    5.  
    6. Q = [Q_angle 0; 0 Q_bias];
    7.  
    8. pitchKF1 = zeros(m,1);
    9. pitchMeasure1 = zeros(m,1);
    10. bias = zeros(m,1);
    11.  
    12. P = [0 0
    13.      0 0];
    14.  
    15. C = [1 0]; 
    16.  
    17.  
    18. for i = 2:m
    19.     
    20.     pitchMeasure1(i) = atan2(-ax(i),-az(i));
    21.  
    22.     A = [1 -dt; 0 1];
    23.  
    24.     %Predicted state estimate x = F.x + B.u
    25.     pitchKF1(i) = pitchKF1(i-1) + dt * (gy(i) - bias(i-1));
    26.     bias(i) = bias(i-1);
    27.  
    28.     %Predicted estimate covariance P = F.P.F' + Q
    29.     P = A*P*A' + Q;
    30.  
    31.     %Innovation y = z - H.x
    32.     y = pitchMeasure1(i) - pitchKF1(i);
    33.  
    34.     %Innovation covariance S = H.P.H' + R
    35.     S = C*P*C' + R_angle;
    36.  
    37.     %Optimal kalman gain K = P.H'/S
    38.     K = P * C' / S;
    39.  
    40.     %Updated state estimate x=x + K.y
    41.     pitchKF1(i) = pitchKF1(i) + K(1)*y;
    42.     bias(i) = bias(i) + K(2)*y;
    43.  
    44.     %Updated estimate covariance P = (I - K.H).P
    45.     P = ([1 0; 0 1] - [K(1); K(2)] * C) * P;
    46. end
    47.  
    48. figure(7)
    49. plot(t1,pitchKF1,t1,pitch,'r');title('单轴卡尔曼模型结果验证');xlabel('Time(s)');grid;
    50. legend('单轴卡尔曼','参考角度')

    仿真结果：

    放大图像：

    可以看出，此算法基本能得到正确结果，除了在某些地方跟踪不好，具体原因后面再讲。

    模型二：

    状态量分别是俯仰角、滚转角以及对应的角速度偏移。与上面的模型相比，这里我们将水平方向的两轴姿态合并在一起，这样易于代码实现，否则在软件中需针对三个轴进行三次调用。

    模型如下：

    代码：

    1. %两轴kalman  模型2
    2. pitchKF2 = zeros(m,1);
    3. rollKF2 = zeros(m,1);
    4. bp = zeros(m,1);
    5. bq = zeros(m,1);
    6. pitchMeasure2 = zeros(m,1);
    7. rollMeasure2 = zeros(m,1);
    8. P_out = zeros(m,4,4);
    9.  
    10. Q_pitch = 0.001; %Calculated from data
    11. Q_roll = 0.001; %Calculated from data
    12. Q_bp = 0.1;
    13. Q_bq = 0.1;
    14. R_pitch = 5; %Accel variance, calculated from data
    15. R_roll = 5; %Accel variance, calculated from data
    16.  
    17. Q = [Q_pitch 0     0    0
    18.      0     Q_roll 0    0
    19.      0     0     Q_bp 0
    20.      0     0     0    Q_bq];
    21.  
    22. R = [R_pitch 0     0 0
    23.      0     R_pitch 0 0
    24.      0     0     0 0
    25.      0     0     0 0];
    26.  
    27. P = [0 0 0 0
    28.      0 0 0 0
    29.      0 0 10000 0
    30.      0 0 0 10000];
    31.  
    32. H = [1 0 0 0
    33.      0 1 0 0
    34.      0 0 0 0
    35.      0 0 0 0];
    36.  
    37. I = [1 0 0 0
    38.      0 1 0 0
    39.      0 0 1 0
    40.      0 0 0 1];
    41.  
    42. x = [gy(1)
    43.      gx(1)
    44.      0
    45.      0];
    46. for i = 1:m
    47.     
    48.      pitchMeasure2(i) = atan2(-ax(i),-az(i));
    49.      rollMeasure2(i) = atan2(ay(i),-az(i));
    50.     
    51.      F = [1 0 -dt 0
    52.          0 1 0 -dt
    53.          0 0 1  0
    54.          0 0 0  1];
    55.     
    56.     B = [dt 0  0 0
    57.          0  dt 0 0
    58.          0  0  0 0
    59.          0  0  0 0];
    60.     
    61.     u = [gy(i)
    62.          gx(i)
    63.          0
    64.          0];
    65.     
    66.     z = [pitchMeasure2(i)
    67.          rollMeasure2(i)
    68.          0
    69.          0];
    70.     
    71.     %Predicted state estimate
    72.     x = F*x + B*u;
    73.     
    74.     %Predicted estimate covariance
    75.     P = F*P*F' + dt*Q;
    76.     
    77.     %Measurement residual
    78.     y = z - H*x;
    79.     
    80.     %Residual covariance
    81.     S = H*P*H' + R;
    82.     
    83.     %Optimal Kalman gain
    84.     K = P*H'*pinv(S);
    85.     
    86.     %Updated state estimate
    87.     x = x + K*y;
    88.     
    89.     %Updated estimate covariance
    90.     P = (I - K*H)*P;
    91.     
    92.     pitchKF2(i) = x(1);
    93.     rollKF2(i) = x(2);
    94.     bp(i) = x(3);
    95.     bq(i) = x(4);
    96.     P_out(i,1:4,1:4) = P(1:4,1:4);
    97. end
    100. figure(8)
    101. subplot(211)
    102. plot(t1,rollKF2,t1,roll,'r');title('roll 角度对比');xlabel('Time(s)');grid;
    103. legend('两轴卡尔曼','参考角度')
    104. subplot(212)
    105. plot(t1,pitchKF2,t1,pitch,'r');title('pitch 角度对比');xlabel('Time(s)');grid;
    106. legend('两轴卡尔曼','参考角度')

    仿真结果：

    目前为止，我们介绍的都是线性模型的姿态估计。以卡尔曼为例，这两种算法基本能满足使用要求，但存在不足。由于线性模型的关系，在只有一个轴向运动的时候，效果还不错，简单来说，比如一个运动同时包含了两个轴，这时候得到的结果就不准确了，上述的一些跟踪不好的地方就是因为此时yaw有运动，所以计算的结果受到了影响。

    所以，实际飞行器的运动为非线性模型，我们也要考虑非线性模型的姿态估计。

    6、mahony算法

    实际在飞控应用时，以上所描述的线性互补很难满足使用要求，因此要求更佳的算法。常见的有mahony，属于非线性互补滤波。算法原理：根据加速度计和地磁计的数据，转换到地理坐标系后，与对应参考的重力向量和地磁向量进行求误差，这个误差用来校正陀螺仪的输出，然后用陀螺仪数据进行四元数更新，再转换到欧拉角。

    具体步骤如下：

    代码：

    1. %mahony
    2. halfT = 0.5 * dt;
    3. Kp = 0.25;
    4. Ki = 0.0;
    5.  
    6. exInt = zeros(m,1);							 
    7. eyInt = zeros(m,1);
    8. ezInt = zeros(m,1); 
    9.  
    10. q0 = zeros(m,1);
    11. q1 = zeros(m,1);
    12. q2 = zeros(m,1);
    13. q3 = zeros(m,1);
    14. q0q0 = zeros(m,1);
    15. q0q1 = zeros(m,1);
    16. q0q2 = zeros(m,1);
    17. q0q3 = zeros(m,1);
    18. q1q1 = zeros(m,1);
    19. q1q2 = zeros(m,1);
    20. q1q3 = zeros(m,1);
    21. q2q2 = zeros(m,1);
    22. q2q3 = zeros(m,1);
    23. q3q3 = zeros(m,1);
    24.  
    25. q0(1) = 1;
    26.  
    27. rollmahony = zeros(m,1);
    28. pitchmahony = zeros(m,1);
    29. yawmahony = zeros(m,1);
    30.  
    31. az = -az;
    32.  
    33.  
    34. for i=2:m
    35.   
    36.     q0q0(i)=q0(i-1)*q0(i-1);
    37.     q0q1(i)=q0(i-1)*q1(i-1);
    38.     q0q2(i)=q0(i-1)*q2(i-1);
    39.     q0q3(i)=q0(i-1)*q3(i-1);
    40.     q1q1(i)=q1(i-1)*q1(i-1);
    41.     q1q2(i)=q1(i-1)*q2(i-1);
    42.     q1q3(i)=q1(i-1)*q3(i-1);
    43.     q2q2(i)=q2(i-1)*q2(i-1);
    44.     q2q3(i)=q2(i-1)*q3(i-1);
    45.     q3q3(i)=q3(i-1)*q3(i-1);
    46.  
    47.     norm = sqrt(mx(i)*mx(i)+my(i)*my(i)+mz(i)*mz(i));
    48.     mx(i)=mx(i)/norm;
    49.     my(i)=my(i)/norm;
    50.     mz(i)=mz(i)/norm;
    51.  
    52.     hx = 2.0 * (mx(i) * (0.5 - q2q2(i) - q3q3(i)) + my(i) * (q1q2(i) - q0q3(i)) + mz(i) * (q1q3(i) + q0q2(i)));  
    53.     hy = 2.0 * (mx(i) * (q1q2(i) + q0q3(i)) + my(i) * (0.5 - q1q1(i) - q3q3(i)) + mz(i) * (q2q3(i) - q0q1(i)));  
    54.     bx = sqrt(hx * hx + hy * hy);  
    55.     ez_ef = -hy * bx;
    56.     
    57.     mag_ex = 2.0 * (q1q3(i) - q0q2(i)) * ez_ef;
    58.     mag_ey = 2.0 * (q2q3(i) + q0q1(i)) * ez_ef;
    59.     mag_ez = (1.0 - 2.0 * q1q1(i) - 2.0 * q2q2(i)) * ez_ef;
    60.     
    61.     norm=sqrt(ax(i)*ax(i)+ay(i)*ay(i)+az(i)*az(i));%把加计的三维向量转成单位向量
    62.     ax(i)=ax(i)/norm;
    63.     ay(i)=ay(i)/norm;
    64.     az(i)=az(i)/norm;
    65.     
    66.     halfex = (ay(i)*(1.0 - 2.0*q1q1(i) - 2.0*q2q2(i)) - az(i)*(2.0*(q2q3(i)+q0q1(i))));                          					
    67.     halfey = (az(i)*(2.0*(q1q3(i)-q0q2(i))) - ax(i)*(1.0 - 2.0*q1q1(i) - 2.0*q2q2(i)));
    68.     halfez = (ax(i)*(2.0*(q2q3(i)+q0q1(i))) - ay(i)*(2.0*(q1q3(i)-q0q2(i))));
    69.     
    70.     ex = halfex;% + mag_ex;
    71.     ey = halfey;% + mag_ey;
    72.     ez = halfez;% + mag_ez;
    73.     
    74.     exInt(i) = exInt(i) + ex* Ki* dt;								 
    75.     eyInt(i) = eyInt(i) + ey* Ki* dt;
    76.     ezInt(i) = ezInt(i) + ez* Ki* dt; 
    77.  
    78.     gx(i) = (gx(i) + Kp*ex + exInt(i))*halfT;					   							
    79.     gy(i) = (gy(i) + Kp*ey + eyInt(i))*halfT;
    80.     gz(i) = (gz(i) + Kp*ez + ezInt(i))*halfT;
    81.  
    82.     q0(i) = q0(i-1) + (-q1(i-1)*gx(i)  - q2(i-1)*gy(i) - q3(i-1)*gz(i));%四元数微分方程
    83.     q1(i) = q1(i-1) + ( q0(i-1)*gx(i) + q2(i-1)*gz(i) - q3(i-1)*gy(i));
    84.     q2(i) = q2(i-1) + ( q0(i-1)*gy(i)  - q1(i-1)*gz(i) + q3(i-1)*gx(i));
    85.     q3(i) = q3(i-1) + ( q0(i-1)*gz(i) + q1(i-1)*gy(i)  - q2(i-1)*gx(i));
    86.  
    87.     norm = sqrt(q0(i)*q0(i) + q1(i)*q1(i) + q2(i)*q2(i) + q3(i)*q3(i));%四元数规范化
    88.     q0(i) = q0(i) / norm;
    89.     q1(i) = q1(i) / norm;
    90.     q2(i) = q2(i) / norm;
    91.     q3(i) = q3(i) / norm;
    92.  
    93.     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
    94.     pitchmahony(i)  = (asin(2 * q0(i) * q2(i) - 2 * q1(i)* q3(i))); % pitch
    95.     yawmahony(i)    = atan2(2 * q1(i) * q2(i) + 2 * q0(i) * q3(i) , -2 *q2(i) * q2(i) - 2 * q3(i) * q3(i) +1);
    96. end
    97.  
    98.  
    99. figure(9)
    100. subplot(311)
    101. plot(t1,rollmahony,t1,roll,'r');title('roll 角度对比');xlabel('Time(s)');grid;
    102. legend('mahony','参考角度')
    103. subplot(312)
    104. plot(t1,pitchmahony,t1,pitch,'r');title('pitch 角度对比');xlabel('Time(s)');grid;
    105. legend('mahony','参考角度')
    106. subplot(313)
    107. plot(t1,yawmahony,t1,yaw,'r');title('yaw 角度对比');xlabel('Time(s)');grid;
    108. legend('mahony','参考角度')

    仿真结果：

    图中可以看出，算法结果基本能跟踪参考角度，性能较好。实际工程中，我们也常常使用mahony算法进行姿态估计。

    7、扩展卡尔曼滤波

    由于是非线性模型，所以在卡尔曼的基础上，我们进行改进，采用扩展卡尔曼滤波算法。

    模型一：

    PX4飞控早期的attitude_estimator_ekf版本。

    此为开源代码，读者可自行进行移植测试。如对理论不理解的，可参见视频教程。

    模型二：

    参考书目

    Nonami K, Kendoul F, Suzuki S, et al. Autonomous Flying Robots: Unmanned Aerial Vehicles and Micro Aerial Vehicles[M]. Springer Publishing Company, Incorporated, 2010.

    第10章

    这里笔者进行了修改，将7状态量缩减成4状态量，只更新四元数。

    代码：

    1. p=rollrate;
    2. q=pitchrate;
    3. r=yawrate;
    4.  
    5. %ACC的测量噪声
    6. var_ax=5;
    7. var_ay=5;
    8. var_az=5;
    9. %mag的测量噪声
    10. var_mx=50;
    11. var_my=50;
    12. var_mz=50;
    13. %状态变量的过程噪声
    14. P1=[1,1,1,1];
    15. P=diag(P1);
    16.  
    17. x_e=[1;0;0;0];
    18. Pitch = zeros(m,1);
    19. Roll = zeros(m,1);
    20. Yaw = zeros(m,1);
    21.  %状态转移矩阵
    22. F = eye(4,4);
    23. %卡尔曼增益矩阵
    24. K = zeros(4,3);
    25. %观测矩阵
    26. H = zeros(3,4);
    27. Hdt=0.5*dt;
    28. Q1 = 0.00001;%Q矩阵中的数也是经验值得到，需要在考虑q中的参数
    29. Q2 = 0.01;
    30.  
    31. R=zeros(6,6);
    32. R(1,1)=var_ax;
    33. R(2,2)=var_ay;
    34. R(3,3)=var_az;
    35. R(4,4)=var_mx;
    36. R(5,5)=var_my;
    37. R(6,6)=var_mz;
    38.  
    39. % R=zeros(3,3);
    40. % R(1,1)=var_ax;
    41. % R(2,2)=var_ay;
    42. % R(3,3)=var_az;
    43.  
    44. for i=1:m
    45.     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));
    46.     for n=1:4
    47.         x_e(n,1)=x_e(n,1)*norm;
    48.     end
    49.     
    50.     %状态转移矩阵
    51.     F = [              1,  -Hdt*(p(i)),  -Hdt*(q(i)),  -Hdt*(r(i));
    52.         Hdt*(p(i)),                1,  Hdt*(r(i)), -Hdt*(q(i));
    53.         Hdt*(q(i)), -Hdt*(r(i)),                1,  Hdt*(p(i));
    54.         Hdt*(r(i)),  Hdt*(q(i)), -Hdt*(p(i)),                1;];
    55.  
    56.     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);
    57.     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);
    58.     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);
    59.     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);
    60.  
    61.     for j=1:4
    62.         x_e(j,1)=dTemp(j);
    63.     end
    64.     %协方差一步预测更新
    65.     P=F*P*F';
    66.     %加入噪音项
    67.     P(1,1)=P(1,1)+Q1;
    68.     P(2,2)=P(2,2)+Q1;
    69.     P(3,3)=P(3,3)+Q1;
    70.     P(4,4)=P(4,4)+Q1;
    72.     
    73.     y(1,1)= -2.0*(x_e(2,1)*x_e(4,1)-x_e(1,1)*x_e(3,1));
    74.     y(2,1)= -2.0*(x_e(1,1)*x_e(2,1)+x_e(3,1)*x_e(4,1));
    75.     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));
    76.     %测量的Acc数据
    77.     norm=sqrt(ax(i)*ax(i)+ay(i)*ay(i)+az(i)*az(i));%把加计的三维向量转成单位向量
    78.     ax(i)=ax(i)/norm;
    79.     ay(i)=ay(i)/norm;
    80.     az(i)=az(i)/norm;
    81.     y_s(1,1) = -ax(i);
    82.     y_s(2,1) = -ay(i);
    83.     y_s(3,1) = az(i);
    84.     
    85.     norm = sqrt(mx(i)*mx(i)+my(i)*my(i)+mz(i)*mz(i));
    86.     mx(i)=mx(i)/norm;
    87.     my(i)=my(i)/norm;
    88.     mz(i)=mz(i)/norm;
    89.     y_s(4,1) = mx(i);
    90.     y_s(5,1) = my(i);
    91.     y_s(6,1) = mz(i);
    93.     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)));  
    94.     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)));  
    95.     bx = sqrt(hx * hx + hy * hy);  
    96.     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)));
    97.      
    98.     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));
    99.     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));
    100.     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));
    102.         %观测矩阵更新
    103. %         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);
    104. %             -2.0*x_e(2,1), -2.0*x_e(1,1), -2.0*x_e(4,1), -2.0*x_e(3,1);
    105. %             -2.0*x_e(1,1),  2.0*x_e(2,1),  2.0*x_e(3,1), -2.0*x_e(4,1);]; 
    106.         
    107.         H = [ 2*x_e(3,1), -2*x_e(4,1),  2*x_e(1,1), -2*x_e(2,1);
    108.             -2*x_e(2,1), -2*x_e(1,1), -2*x_e(4,1), -2*x_e(3,1);
    109.             -2*x_e(1,1),  2*x_e(2,1),  2*x_e(3,1), -2*x_e(4,1);
    110.             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);
    111.             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);
    112.             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)]; 
    113.         
    114.         %计算卡尔曼增益
    115.         K=P*H'*pinv(H*P*H'+R);
    116.         %状态估计
    117.         x_e=x_e+K*(y_s-y);
    118.         %x_e=x_e;
    119.         P=P-K*H*P;
    121.     
    122.         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));
    123.         for n=1:4
    124.             x_e(n,1)=x_e(n,1)*norm;
    125.         end
    126.         %四元数转欧拉角
    127.         Pitch(i) = asin(2*(x_e(1,1)*x_e(3,1)- x_e(2,1)*x_e(4,1)));
    128.         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)));
    129.         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)));
    131. end   
    134. figure(11)
    135. subplot(311)
    136. plot(t1,Roll,t1,roll,'r');title('roll 角度对比');xlabel('Time(s)');grid;
    137. legend('EKF','参考角度')
    138. subplot(312)
    139. plot(t1,Pitch,t1,pitch ,'r');title('pitch 角度对比');xlabel('Time(s)');grid;
    140. legend('EKF','参考角度')
    141. subplot(313)
    142. plot(t1,Yaw,t1,yaw,'r');title('yaw 角度对比');xlabel('Time(s)');grid;
    143. legend('EKF','参考角度')

    仿真结果：

    Yaw部分是因为参考角度去掉了偏移，只有相对角度，所以实际是正确的跟踪。上图表明，EKF能达到较好的效果，满足使用要求。

    至此，我们已经总结了在姿态估计中常用的几种算法，每个算法的不足与优点也大概介绍了。实际在应用时，并不一定是理论越复杂就一定好用，比如EKF，如果模型维数过多，则计算量大大增加，且增加了算法发散的风险，所以工程应用，只选用满足要求的最佳算法。

    总结一下：

    1、 坐标系对应是姿态估计非常重要的一环，特别是移植；

    2、 如何调参？

    工程上进行算法开发时，笔者建议的流程是先分析问题，选用相应的算法，并进行设计，取飞行数据，然后在matlab仿真环境下进行仿真调试，最后转换成嵌入式代码实现功能。这样做的好处是，如果在嵌入式环境下进行大量的调参时，非常繁琐，不方便以及不直观。当然，现在有基于模型的开发方式，这里不做过多叙述。具体如何调参，可参见视频教程；

    3、 工程应用时，要考虑震动对数据以及算法的影响，动加速度对重力参考的影响，外界地磁干扰对地磁参考的影响，如何在算法中避免这些情况，或者说出现这些情况下如何保证算法不崩溃，即鲁棒性；

    4、 没有绝对的最好的算法理论，如何使用，达到最佳效果往往是算法工程师要做的；

    5、如果想更多的了解状态估计的理论知识，可参考等其他资料。

  • 相关阅读:
    【LeetCode动态规划#13】买卖股票含冷冻期（状态众多，比较繁琐）、含手续费
    yolov6训练自己的数据记录+yolov5对比测试
    QTday2
    Spring6整合JUnit5
    jmeter全局变量有的线程组引用不成功
    Scala003--Scala中的运算符及注释
    基于BP神经网络的含水率预测模型matlab仿真
    我们离成为C++、C#、MySQL之父有多远？
    uni-app 使用 scss 实现推荐标签区域显示效果
    电机控制::理论分析::延迟环节对系统的影响
  • 原文地址：https://blog.csdn.net/boboelec/article/details/133266658