RflySim | 滤波器设计实验二

本讲是关于无人机滤波器，其中包括无人机滤波器简介、测量原理、线性互补滤波器设计、线性互补滤波器参数分析、卡尔曼滤波器设计等。

滤波器设计实验2

卡尔曼滤波器是一种递推线性最小方差估计算法，它的最优估计需满足以下三个条件：

1）无偏性：即估计值的期望等于状态的真值；

2）估计的方差最小；

3）实时性。

假设线性离散系统模型如下：

式中，过程噪声wk-1和观测噪声vk的统计特性为：

卡尔曼滤波算法可总结为如下：

卡尔曼滤波器在进行滤波器估计的同时还产生了误差协方差阵，它可以用于表征估计精度，同时也能用于传感器的健康评估。一般来说，采样周期合理情况下，连续系统可观，离散化的系统也会可观。然而有时候采样周期选择不当，系统可能失去可控性及可观性。因此原则上应该检查离散化系统的可观性。 HkPk|k-1HTk+Pk|k-1需要是H非奇异的，否则kk = Pk|k-1HTk（HkPk|k-1HTk+Pk)-1无法实现。如果(ФK,K-1,Hk) 不可观，那么卡尔曼滤波器仍然可以运行，只不过不可观的模态没有进行修正，只是递推罢了。极端情况 Hk=0m`n ，那么

卡尔曼滤波器实现见文件“Attitude_ekf.m”，其主要部分如下

function [ x_aposteriori, P_aposteriori, roll, pitch] =

Attitude_ekf( dt, z, q, r, x_aposteriori_k, P_aposteriori_k)

%函数描述:

% 状态估计的拓展卡尔曼滤波方法

%输入:

% dt: 更新周期

% z: 测量值

% q:系统噪声，r:测量噪声

% x_aposteriori_k: 上一时刻的状态估计

% P_aposteriori_k: 上一时刻估计协方差

%输出:

% x_aposteriori：当前时刻的状态估计

% P_aposteriori：当前时刻的估计协方差

% roll,pitch：欧拉角，单位: rad

w_m = z(1:3); %角速度测量值

a_m = z(4:6); %加速度测量值

g = norm(a_m,2);   %重力加速度

%   w_x_=[ 0,-(wz-bzg, wy-byg;

%         wz-bzg, 0 ,-(wx-bxg);

%         -(wy-byg), wx-bxg, 0];

w_x_ = [0, -(w_m(3) - x_aposteriori_k(3)), w_m(2) -x_aposteriori_k(2);

      w_m(3) - x_aposteriori_k(3), 0, -(w_m(1) - x_aposteriori_k(1));

      -(w_m(2) - x_aposteriori_k(2)), w_m(1) - x_aposteriori_k(1), 0];

  bCn = eye(3, 3) - w_x_*dt;

% 预测

% 更新先验状态矩阵

x_apriori = zeros(1, 6);

x_apriori(1: 3) = x_aposteriori_k(1 : 3); %角速度漂移

x_apriori(4 : 6) = bCn*x_aposteriori_k(4 : 6);   %加速度归一化值

%[x]x

x_aposteriori_k_x = [0, -x_aposteriori_k(6), x_aposteriori_k(5);

                        x_aposteriori_k(6), 0, -x_aposteriori_k(4);

                      -x_aposteriori_k(5), x_aposteriori_k(4), 0];

% 更新状态转移矩阵

PHI = [eye(3, 3), zeros(3, 3);

         -x_aposteriori_k_x*dt, bCn];

GAMMA = [eye(3, 3)*dt, zeros(3, 3); % 噪声驱动阵

      zeros(3, 3), -x_aposteriori_k_x*dt];

Q = [eye(3, 3)*q(1), zeros(3, 3);

   zeros(3, 3), eye(3, 3)*q(2)];

% 预测误差协方差矩阵

P_apriori = PHI*P_aposteriori_k*PHI' + GAMMA*Q*GAMMA';

% 更新

R = eye(3, 3)*r(1);

H_k = [zeros(3, 3), -g*eye(3, 3)];

%卡尔曼增益

K_k = (P_apriori*H_k')/(H_k*P_apriori*H_k' + R);

% 状态估计矩阵

x_aposteriori = x_apriori' + K_k*(a_m - H_k*x_apriori');

% 估计误差协方差

P_aposteriori = (eye(6, 6) - K_k*H_k)*P_apriori;

% 计算滚转角和俯仰角，分别对应roll,pitch

k = x_aposteriori(4 : 6) /norm(x_aposteriori(4 : 6), 2);

roll = atan2(k(2), k(3));   % 滚转角

pitch = -asin(k(1));     %俯仰角

end