1.算法描述

卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器)，它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态。这种滤波方法以它的发明者鲁道夫·E·卡尔曼（Rudolf E. Kalman）命名。卡尔曼最初提出的滤波理论只适用于线性系统。Bucy，Sunahara等人提出并研究了扩展卡尔曼滤波(EKF)，将卡尔曼滤波理论进一步应用到非线性领域。

扩展卡尔曼滤波（Extended Kalman Filter，EKF）是标准卡尔曼滤波在非线性情形下的一种扩展形式，EKF算法是将非线性函数进行泰勒展开，省略高阶项，保留展开项的一阶项，以此来实现非线性函数线性化，最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值,对信号进行滤波。
扩展卡尔曼滤波EKF的状态转移方程和观测方程为：

EKF和KF的区别如下：

姿态解算就是通过融合传感器数据解算出姿态角。姿态角是俯仰角（pitch）、滚转角（roll）和偏航角（yaw）的合称，此文分别使用 α , β , γ \alpha, \beta, \gammaα,β,γ 表示。

俯仰角
俯仰角是无人机机体系 x 轴与水平面夹角，也即机体系与航向系 x 轴的夹角，机头上仰为正，范围 α ∈ [ − π / 2 , π / 2 ] \alpha \in [-\pi/2, \pi/2]α∈[−π/2,π/2]。

滚转角
滚转角是无人机机体系 y 轴与水平面夹角，也即机体系与航向系 y 轴的夹角，机身左升右降为正，范围 β ∈ [ − π , π ] \beta \in [-\pi, \pi]β∈[−π,π]。

偏航角
偏航角是无人机机体系 x 轴在水平面投影与地球系 x 轴（正北方）的夹角，也即航向系 x 轴与地球系 x 轴夹角。俯视机身，顺时针方向（往东）角度递增，逆时针方向角度递减，范围 γ ∈ [ − π , π ] \gamma \in [-\pi, \pi]γ∈[−π,π]。

可见，引入航向系之后可以更加方便地定义姿态角。注意与下面的欧拉角作对比，欧拉角和姿态角不是同样概念，这也是这里使用 α , β , γ \alpha, \beta, \gammaα,β,γ 而不是更常见的 θ , ϕ , ψ \theta, \phi, \psiθ,ϕ,ψ 的原因，后者用于表示欧拉角。

2.仿真效果预览

matlab2022a仿真结果如下：

3.MATLAB核心程序

 
 
%常系数
L= 0.3875;  %单位(m)
Ix = 0.05887;  %单位(kg·m^2)
Iy = 0.05887;
Iz = 0.13151;
g = 9.81; %单位(N/kg)
 
%动力学方程的常系数
a1 = -(Iy - Iz)/Ix;
a2 = -(Iz - Ix)/Iy;
a3 = -(Ix - Iy)/Iz;  
b1 = L/Ix;
b2 = L/Iy;
b3 = 1/Iz;
 
Ts = 0.1;                    %采样时间
t = 5;                       %仿真时间
len = fix(t/Ts);            %仿真步数
n = 6;                        %状态维度
w = 0.1;                     %过程标准差
v = 0.5;                      %测量标准差
Q = w^2*eye(n);        %过程方差
R = v^2;                    %测量值的方差
 
h=@(x)[x(2);x(4);x(6)];                  %测量方程
s=[1;2;3;3;2;1];                            %初始状态
x=s+w*randn(6,1);                      %初始化状态
P = eye(6);                                 %初始化协方差矩阵
xV = zeros(6,len);                       %EKF估计值
sV = zeros(6,len);                       %真实值
zV = zeros(3,len);                       %测量值
 
for k=1:len
  %随机赋值控制量
  u2 = 0.1*randn(1,1);
  u3 = 0.1*randn(1,1);
  u4 = 0.1*randn(1,1);
  
  z = h(s) + v*randn;                     
  sV(:,k)= s;                             %实际状态
  zV(:,k) = z;                           %状态测量值
  
  %状态方程
  f=@(x)[x(1)+Ts*x(2);
           (a1*x(4)*x(6) +b1*u2)*Ts+x(2);
           x(3)+Ts*x(4);
           (a2*x(2)*x(6) +b2*u3)*Ts+x(4);
           x(5)+Ts*x(6);
           (a3*x(2)*x(4) +b3*u4)*Ts+x(6);];  
  
  %一步预测，同时计算f的雅可比矩阵A
  [x1,A]=jaccsd(f,x); 
  
  %过程方差预测
  P=A*P*A'+Q;         
  
  %状态预测，同时计算h的雅可比矩阵H
  [z1,H]=jaccsd(h,x1); 
  
  %计算卡尔曼增益
  K=P*H'/(H*P*H'+R); 
  
  %状态EKF估计值
  x=x1+K*(z-z1);        
  
  %协方差更新
  P=P-K*H*P;          
  
  xV(:,k) = x;          
  
  %更新状态
  s = f(s) + w*randn(6,1);  
end
 
%俯仰角、滚转角、偏航角度值
for k=1:2:5
  figure(); hold on; 
  plot(sV(k,:),'-.'); %画出真实值
  plot(xV(k,:)) %画出最优估计值
  plot(abs(sV(k,:)-xV(k,:)), '--'); %画出误差值
  legend('真实状态', 'EKF最优估计估计值', '误差值');
end
A128