PX4 EKF MATLAB代码

PX4 EKF MATLAB代码参考，建立以角速度，角加速度，重力加速度，磁通量共12阶的数据为状态的状态方程。以角速度、加速度、磁通量建立量测方程进行EKF滤波

function [ x_aposterioriP_aposterioriRotMatrixrollpitchyaw] = AttltitudeEKF（ dtz qrx_aposteriori_kP_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=[ 0wz -wy;
% ? ? ? -wz 0 wx;
% ? ? ? wy -wx 0];
    O=[0x_aposteriori_k（3）-x_aposteriori_k（2）;
      -x_aposteriori_k（3）0x_aposteriori_k（1）;
      x_aposteriori_k（2）-x_aposteriori_k（1）0];
      bCn = eye（33） +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（33）eye（33）*dtzeros（33）zeros（33）;
          zeros（33）eye（33）zeros（33）zeros（33）;
          r_a*dtzeros（33）（eye（33）+O*dt）zeros（33）;
          r_m*dtzeros（33）zeros（33）（eye（33）+O*dt）];
  %预测误差协方差矩阵
  Q=[eye（33）*q（1）zeros（33）zeros（33）zeros（33）;
     zeros（33）eye（33）*q（2）zeros（33）zeros（33）;
     zeros（33）zeros（33）eye（33）*q（3）zeros（33）;
     zeros（33）zeros（33）zeros（33）eye（33）*q（4）];
 P_apriori = A_lin*P_aposteriori_k*A_lin‘+A_lin*Q*A_lin‘;
 

 %卡尔曼增益
 R=[eye（33）*r（1）zeros（33）zeros（33）;
     zeros（33）eye（33）*r（2）zeros（33）;
     zeros（33）zeros（33）eye（33）*r（3）];
 H_k=[eye（33）zeros（33）zeros（33）zeros（33）;
      zeros（33）zeros（33）eye（33）zeros（33）;
      zeros（33）zeros（33）zeros（33）eye（33）];
  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（1212）-K_k*H_k）*P_apriori;%计算方式简单但是容易失去正定性，PX4中使用这种方式
  P_aposteriori=（eye（1212）-K_k*H_k）*P_apriori*（eye（1212）-K_k*H_k）‘+K_k*R*K_k‘;
 

   R=eye（33）*r（1）;
   H_k=[eye（33）zeros（33）zeros（33）zeros（33）];
   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（1212）-K_k*H_k）*P_apriori*（eye（1212）-K_k*H_k）‘+K_k*R*K_k‘;
 
 
     R=[eye（33）*r（1）zeros（33）;
        zeros（33）eye（33）*r（2）];
H_k=[eye（33）zeros（33）zeros（33）zeros（33）;
    zeros（33）zeros（33）eye（33）zeros（33）];
    K_k=（P_apriori*H_k‘）/（H_k*P_apriori*H_k‘+R）;
   x_aposteriori=x_apriori‘+K_k*（z（1:

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       4031  2018-10-29 15:31  PX4EKF  MATLAB代码参考\AttltitudeEKF.m

     目录          0  2019-01-09 15:28  PX4EKF  MATLAB代码参考

----------- ---------  ---------- -----  ----

                 4031                    2