四旋翼姿态解算--互补滤波和拓展卡尔曼

小记一下做的四旋翼姿态解算。

关于利用互补滤波进行姿态解算的文章随便一搜就有很多,就不列出来了。关于卡尔曼的理解和原理,觉得这篇挺通俗易懂的,(二)中还附有matlab代码:

卡尔曼滤波 -- 从推导到应用:http://blog.csdn.net/heyijia0327/article/details/17487467

关于用EKF进行姿态解算,推荐这篇博文,博主对代码的实现原理进行了详细的说明和注释(博主使用的代码是比较老的本版了firmware-1.0.0-rc3及以前版本,之后的版本中应该是加了二阶EKF,可以选择是使用一阶还是二阶。另外有attitude_estimator_ekf的最新版本是firmware-1.4.1,之后的版本放在example中了):

PX4飞控中利用EKF估计姿态角代码详解:http://blog.csdn.net/lizilpl/article/details/45542907

下面根据上面的博文和秦永元的《卡尔曼滤波与组合导航原理(第三版)》做一下总结。

基本概念:


EKF:





    估计均方误差有三种形式,实际常使用第一种;第二种运算量小,但是在计算中积累误差容易使Pk失去非负定性甚至对称性;如果在滤波初始时刻对被估计量特性缺乏了解,滤波初值P0选取比较盲目,选的很大,采用第三种。增益应该是这样:


矩阵除法有左除右除之分,写成上面那样看着爽一点。








下面贴上matlab仿真程序:

function [ x_aposteriori,P_aposteriori,RotMatrix,roll,pitch,yaw] = AttltitudeEKF( updateVect,dt,z ,q,r,x_aposteriori_k,P_aposteriori_k)
%AttltitudeEKF 状态估计的拓展卡尔曼滤波方法
%输入:
%   updateVect:指示哪些数据进行了更新
%   dt:更新周期
%   z:测量值
%   q:系统噪声,r:测量噪声
%   H_k:测量矩阵
%   x_aposteriori_k:上一时刻的状态估计
%   P_aposteriori_k:上一时刻估计协方差
%输出:
%   x_aposteriori:当前时刻的状态估计
%   P_aposteriori:当前时刻的估计协方差
%   RotMatrix:旋转矩阵
%   roll、pitch、yaw:欧拉角

%   O=[ 0,wz, -wy;
%       -wz, 0 ,wx;
%       wy, -wx, 0];
    O=[0,x_aposteriori_k(3),-x_aposteriori_k(2);
      -x_aposteriori_k(3),0,x_aposteriori_k(1);
       x_aposteriori_k(2),-x_aposteriori_k(1),0];
    bCn = eye(3,3) +O*dt;%旋转矩阵
%更新先验状态矩阵
  x_apriori(1:3) = x_aposteriori_k(1:3) + x_aposteriori_k(4:6)*dt;%角速度
  x_apriori(4:6) = x_aposteriori_k(4:6);%角加速度
  x_apriori(7:9) = bCn*x_aposteriori_k(7:9);%加速度
  x_apriori(10:12) = bCn*x_aposteriori_k(10:12);%磁场
  %更新状态转移矩阵
%   r_a=[ 0,  -az, ay;    r_m=[ 0,  -mz, my;
%         az,  0,  -ax;         mz,  0,  -mx;
%        -ay, ax,  0];         -my,  mx,  0];
  r_a = [0,-x_aposteriori_k(9),x_aposteriori_k(8);
         x_aposteriori_k(9),0,-x_aposteriori_k(7);
         -x_aposteriori_k(8),x_aposteriori_k(7),0];
  r_m = [0, -x_aposteriori_k(12),x_aposteriori_k(11);
         x_aposteriori_k(12),0,-x_aposteriori_k(10);
         -x_aposteriori_k(11),x_aposteriori_k(10),0];
 A_lin = [eye(3,3),eye(3,3)*dt,zeros(3,3),zeros(3,3);
          zeros(3,3),eye(3,3),zeros(3,3),zeros(3,3);
          r_a*dt,zeros(3,3),(eye(3,3)+O*dt),zeros(3,3);
          r_m*dt,zeros(3,3),zeros(3,3),(eye(3,3)+O*dt)];
  %预测误差协方差矩阵
  Q=[eye(3,3)*q(1),zeros(3,3),zeros(3,3),zeros(3,3);
     zeros(3,3),eye(3,3)*q(2),zeros(3,3),zeros(3,3);
     zeros(3,3),zeros(3,3),eye(3,3)*q(3),zeros(3,3);
     zeros(3,3),zeros(3,3),zeros(3,3),eye(3,3)*q(4)];
 P_apriori = A_lin*P_aposteriori_k*A_lin'+A_lin*Q*A_lin';

 if((updateVect(1)~=0) && (updateVect(2)~=0) && updateVect(3)~=0)
 %卡尔曼增益
 R=[eye(3,3)*r(1),zeros(3,3),zeros(3,3);
     zeros(3,3),eye(3,3)*r(2),zeros(3,3);
     zeros(3,3),zeros(3,3),eye(3,3)*r(3)];
 H_k=[eye(3,3),zeros(3,3),zeros(3,3),zeros(3,3);
      zeros(3,3),zeros(3,3),eye(3,3),zeros(3,3);
      zeros(3,3),zeros(3,3),zeros(3,3),eye(3,3)];
  K_k=(P_apriori*H_k')/(H_k*P_apriori*H_k'+R);
  %状态估计矩阵
  x_aposteriori=x_apriori'+K_k*(z - H_k*x_apriori');
  %估计误差协方差
 % P_aposteriori=(eye(12,12)-K_k*H_k)*P_apriori;%计算方式简单但是容易失去正定性,PX4中使用这种方式
  P_aposteriori=(eye(12,12)-K_k*H_k)*P_apriori*(eye(12,12)-K_k*H_k)'+K_k*R*K_k';

elseif((updateVect(1)~=0) && (updateVect(2)==0) && updateVect(3)==0)
   R=eye(3,3)*r(1);
   H_k=[eye(3,3),zeros(3,3),zeros(3,3),zeros(3,3)];
   K_k=(P_apriori*H_k')/(H_k*P_apriori*H_k'+R);
   x_aposteriori=x_apriori'+K_k*(z(1:3) - H_k*x_apriori');
   P_aposteriori=(eye(12,12)-K_k*H_k)*P_apriori*(eye(12,12)-K_k*H_k)'+K_k*R*K_k';

 elseif((updateVect(1)~=0) && (updateVect(2)~=0) && updateVect(3)==0) 
     R=[eye(3,3)*r(1),zeros(3,3);
        zeros(3,3),eye(3,3)*r(2)];
H_k=[eye(3,3),zeros(3,3),zeros(3,3),zeros(3,3);
    zeros(3,3),zeros(3,3),eye(3,3),zeros(3,3)];
    K_k=(P_apriori*H_k')/(H_k*P_apriori*H_k'+R);
   x_aposteriori=x_apriori'+K_k*(z(1:6) - H_k*x_apriori');
   P_aposteriori=(eye(12,12)-K_k*H_k)*P_apriori*(eye(12,12)-K_k*H_k)'+K_k*R*K_k';
   
  elseif((updateVect(1)~=0) && (updateVect(2)==0) && updateVect(3)~=0) 
     R=[eye(3,3)*r(1),zeros(3,3);
         zeros(3,3),eye(3,3)*r(3)];
    H_k=[eye(3,3),zeros(3,3),zeros(3,3),zeros(3,3);
    zeros(3,3),zeros(3,3),zeros(3,3),eye(3,3)];
      K_k=(P_apriori*H_k')/(H_k*P_apriori*H_k'+R);
   x_aposteriori=x_apriori'+K_k*([z(1:3);z(7:9)] - H_k*x_apriori');
   P_aposteriori=(eye(12,12)-K_k*H_k*P_apriori)*(eye(12,12)-K_k*H_k)'+K_k*R*K_k';
 end

  %得到旋转矩阵和角度
  k = -x_aposteriori(7:9) /norm(x_aposteriori(7:9),2);
  i = x_aposteriori(10:12) /norm(x_aposteriori(10:12),2);
  j=cross(k,i);
  j=j/norm(j,2);
  RotMatrix=[i';j';k'];
  
  roll=atan2(RotMatrix(3,2),RotMatrix(3,3))*57.3;
  pitch=-asin(RotMatrix(3,1))*57.3;
  yaw=atan2(RotMatrix(2,1),RotMatrix(1,1))*57.3;
end

上述的EKF本质上就是对加速度计、陀螺仪、磁力计的数据进行滤波。经过滤波的加速度和磁力计数据可以得到旋转矩阵,也就估计出了无人机的姿态。下面是根据传感器采集的一组数据做的仿真。加速度和角速度从MPU6050采集,采样频率200Hz;磁力计数据从HMC5983采集,采集频率为10Hz。EKF的参数采用PX4程序中默认的参数,q=[0.0001,0.08,0.009,0.005],r=[0.0008,1000,100],这个参数中加速度的噪声选的很大,解算的角度平滑,但是延迟大。上面的图是采集手动晃动数据计算出的结果,下面的图是实际飞行过程中采集数据计算的结果。



相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页