四旋翼姿态解算--互补滤波和拓展卡尔曼
小记一下做的四旋翼姿态解算。
关于利用互补滤波进行姿态解算的文章随便一搜就有很多，就不列出来了。关于卡尔曼的理解和原理，觉得这篇挺通俗易懂的，（二）中还附有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],这个参数中加速度的噪声选的很大，解算的角度平滑，但是延迟大。上面的图是采集手动晃动数据计算出的结果，下面的图是实际飞行过程中采集数据计算的结果。
