题外话：虽然导航与控制领域在现代工业中应用广泛，但近些年火热的多旋翼无人机无疑是推动这一领域进一步发展的最重要的推手，如日中天的汽车智能驾驶行业也将使导航和控制领域充分融合计算机视觉（SLAM）、机器学习（道路及障碍物识别）、高端制造业（雷达、摄像头、IMU、GPS、处理器）、通信（车联网、车路协同）等专业方向的研究成果，焕发出更加强大的生命力。
由于大多数的INS/GPS组合导航算法模型都是基于非线性模型，如何有效的融合INS与GPS的测量信息来对当前载体的位置、速度、姿态等信息进行估计成为一个被长期研究的课题。本篇博客试图记录一下使用直接法EKF算法实现INS/GPS松组合的过程，姿态角以欧拉角表示。
本文的INS/GPS组合模型主要参考文献：
Barton, Jeffrey. (2012). Fundamentals of Small Unmanned Aircraft Flight. Johns Hopkins Apl Technical Digest. 31. 132-149.
实验数据来源于之前的博客：非线性滤波——INS/GPS组合导航仿真数据的生成
模型约定
所有坐标系均约定为右手坐标系
航坐标系：NED（north-east-down） | 北东地 | x-y-z
机体坐标系：前-右-下 | x-y-z
欧拉角定义：
- 绕x轴旋转-横滚角 ϕ \phi ϕ
- 绕y轴旋转-俯仰角 θ \theta θ
- 绕z轴旋转-偏航角
ψ
\psi
ψ
旋转顺序（ Z − Y − X Z-Y-X Z−Y−X）：
C n b = ( 1 0 0 0 cos ϕ sin ϕ 0 − sin ϕ cos ϕ ) ( cos θ 0 − sin θ 0 1 0 sin θ 0 cos θ ) ( cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 ) = ( cos θ cos ψ cos θ sin ψ − sin θ cos ψ sin θ sin ϕ − cos ϕ sin ψ cos ϕ cos ψ + sin θ sin ϕ sin ψ cos θ sin ϕ sin ϕ sin ψ + cos ϕ cos ψ sin θ cos ϕ sin θ sin ψ − cos ψ sin ϕ cos θ cos ϕ ) \begin{aligned} &C_{n}^{b}=\left(\begin{array}{ccc} {1} & {0} & {0} \\ {0} & {\cos \phi} & {\sin \phi} \\ {0} & {-\sin \phi} & {\cos \phi} \end{array}\right)\left(\begin{array}{ccc} {\cos \theta} & {0} & {-\sin \theta} \\ {0} & {1} & {0} \\ {\sin \theta} & {0} & {\cos \theta} \end{array}\right)\left(\begin{array}{ccc} {\cos \psi} & {\sin \psi} & {0} \\ {-\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1} \end{array}\right)\\ &=\left(\begin{array}{ccc} {\cos \theta \cos \psi} & {\cos \theta \sin \psi} & {-\sin \theta} \\ {\cos \psi \sin \theta \sin \phi-\cos \phi \sin \psi} & {\cos \phi \cos \psi+\sin \theta \sin \phi \sin \psi} & {\cos \theta \sin \phi} \\ {\sin \phi \sin \psi+\cos \phi \cos \psi \sin \theta} & {\cos \phi \sin \theta \sin \psi-\cos \psi \sin \phi} & {\cos \theta \cos \phi} \end{array}\right) \end{aligned} Cnb=⎝⎛1000cosϕ−sinϕ0sinϕcosϕ⎠⎞⎝⎛cosθ0sinθ010−sinθ0cosθ⎠⎞⎝⎛cosψ−sinψ0sinψcosψ0001⎠⎞=⎝⎛cosθcosψcosψsinθsinϕ−cosϕsinψsinϕsinψ+cosϕcosψsinθcosθsinψcosϕcosψ+sinθsinϕsinψcosϕsinθsinψ−cosψsinϕ−sinθcosθsinϕcosθcosϕ⎠⎞
C n b = ( C b n ) T C_n^b=(C_b^n)^T Cnb=(Cbn)T
模型描述
INS/GPS的松组合模型所使用的传感器有三轴IMU，三轴磁强计和GPS接收机模块，它们分别提供载体在机体坐标系下的加速度信息、角速度信息、航向信息（已经预先基于三轴磁强计解算出航向信息，并减去当地磁偏角）、载体在导航系中的三维位置信息和导航系中的三维速度信息。各个传感器（尤其是IMU和三轴磁强计）需要经过初始校准，GPS接收机模块（以ublox m8n中ubx协议中的pvt为例）配置输出经度、纬度、海拔高度和NED坐标系下的三轴速度。
INS/GPS的松组合模型可以用状态方程和测量方程描述：
-
状态方程是指系统自身的模型，系统可以根据状态方程来预测下一时刻的状态，由IMU参与的组合导航模型中，IMU的测量值起到了驱动状态方程的作用。由于MEMS IMU本身具有一定程度的噪声，对速度、位置等状态的预测会引入致命的累计误差，因此，状态方程仅可以提供较短甚至极短时间内（几十毫秒）的状态估计，长时间单一的依赖状态方程对当前状态进行估计会得到极不可靠的状态估计值。本文中的所要研究的简化版状态模型为
d d t x ˉ ^ = [ ϕ ^ ˙ θ ^ ˙ ψ ^ ˙ P ^ ˙ n P ^ ˙ e h ^ ˙ M S L V ^ ˙ n V ^ ˙ e V ^ ˙ d b ^ ˙ w x b ^ ˙ w y b ^ ˙ w z b ^ ˙ b x b ^ ˙ b y b ^ ˙ b z ] = [ [ 1 sin ϕ ^ tan θ ^ cos ϕ ^ tan θ ^ 0 cos ϕ ^ − sin ϕ ^ 0 sin ϕ ^ sec θ ^ cos ϕ ^ sec θ ^ ] [ ω x , g y r o − b ^ ω x ω y , g y r o − b ^ ω y ω z , g y r o − b ^ ω z ] V ^ n V ^ e − V ^ d C ^ b n [ f x , accel − b ^ a x f y , accel − b ^ a y f z , accel − b ^ a z ] 0 0 0 0 0 0 ] \frac{d}{dt}\bold{\hat{\bar{x}}}=\left[\begin{array}{c} \dot{\hat{\phi}} \\ \dot{\hat{\theta}} \\ \dot{\hat{\psi}} \\ \dot{\hat{P}}_n \\ \dot{\hat{P}}_e \\ \dot{\hat{h}}_{MSL} \\ \dot{\hat{V}}_n \\ \dot{\hat{V}}_e \\ \dot{\hat{V}}_d \\ \dot{\hat{b}}_{w_x} \\ \dot{\hat{b}}_{w_y} \\ \dot{\hat{b}}_{w_z} \\ \dot{\hat{b}}_{b_x}\\ \dot{\hat{b}}_{b_y} \\ \dot{\hat{b}}_{b_z} \\ \end{array}\right]=\left[\begin{array}{c} \left[\begin{array}{ccc} {1} & {\sin \hat{\phi} \tan \hat{\theta}} & {\cos \hat{\phi} \tan \hat{\theta}} \\ {0} & {\cos \hat{\phi}} & {-\sin \hat{\phi}} \\ {0} & {\sin \hat{\phi} \sec \hat{\theta}} & {\cos \hat{\phi} \sec \hat{\theta}} \end{array}\right]\left[\begin{array}{cc} {\omega_{x, g y r o}} - {\hat{b}_{\omega_{x}}} \\ {\omega_{y, g y r o}} - {\hat{b}_{\omega_{y}}} \\ {\omega_{z, g y r o}} - {\hat{b}_{\omega_{z}}} \end{array}\right]\\ {\hat{V}}_n \\ {\hat{V}}_e \\ -{\hat{V}}_d \\ \hat{C}_{b}^{n}\left[\begin{array}{l} {f_{x, \text { accel }}-\hat{b}_{a_{x}}} \\ {f_{y, \text { accel }}-\hat{b}_{a_{y}}} \\ {f_{z, \text { accel }}-\hat{b}_{a_{z}}} \end{array}\right]\\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ \end{array}\right] dtdxˉ^=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡ϕ^˙θ^˙ψ^˙P^˙nP^˙eh^˙MSLV^˙nV^˙eV^˙db^˙wxb^˙wyb^˙wzb^˙bxb^˙byb^˙bz⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡⎣⎡100sinϕ^tanθ^cosϕ^sinϕ^secθ^cosϕ^tanθ^−sinϕ^cosϕ^secθ^⎦⎤⎣⎡ωx,gyro−b^ωxωy,gyro−b^ωyωz,gyro−b^ωz⎦⎤V^nV^e−V^dC^bn⎣⎡fx, accel −b^axfy, accel −b^ayfz, accel −b^az⎦⎤000000⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
可以将其进一步简记为
x ˙ = f ( x ) + w ( t ) , w ( t ) ∼ N ( 0 , Q ) \dot{\bold{x}}=f(\bold{x})+\bold{w}(t), \mathbf{w}(t) \sim N(0, \mathbf{Q}) x˙=f(x)+w(t),w(t)∼N(0,Q)
由于该方程较为基础，本文不再对状态方程的形式进行额外的解读。 -
测量方程是对系统本身从其他传感器对所要估计的状态的直接或者间接测量模型的描述，可以对状态方程推演过程中产生的误差进行修正。本文中的INS/GPS松组合模型将GPS模块的位置、速度和三轴磁强计计算得到的航向角信息作为测量量，方程可以写为
z ˉ = [ ψ m a g P n , G P S P e , G P S h M S L , G P S V n , G P S V e , G P S V d , G P S ] = [ ψ ^ P ^ n P ^ e h ^ M S L V ^ n V ^ e V ^ d ] + w ( t ) \bar{\bold{z}}=\left[\begin{array}{c} {\psi_{{mag}}} \\ {P_{n, { GPS }}} \\ {P_{e, { GPS }}} \\ {h_{{MSL, GPS }}} \\ {V_{{n, GPS }}} \\ {V_{e, { GPS }}} \\ {V_{{d, GPS }}} \end{array}\right]= \left[\begin{array}{c} {\hat{\psi}} \\ {\hat{P}_{n}} \\ {\hat{P}_{e}} \\ {\hat{h}_{{MSL}}} \\ {\hat{V}_{{n}}} \\ {\hat{V}_{e}} \\ {\hat{V}_{{d}}} \end{array}\right]+\bold{w}(t) zˉ=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡ψmagPn,GPSPe,GPShMSL,GPSVn,GPSVe,GPSVd,GPS⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡ψ^P^nP^eh^MSLV^nV^eV^d⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤+w(t)
与状态方程类似，可以将其简写为
z ˉ = h ( x ) + v ( t ) , v ( t ) ∼ N ( 0 , R ) \bar{\bold{z}}=h(\bold{x})+\bold{v}(t), \mathbf{v}(t) \sim N(0, \mathbf{R}) zˉ=h(x)+v(t),v(t)∼N(0,R)
可以直观的看到，本文所要研究的INS/GPS松组合模型中，状态方程为非线性，测量方程为线性。而一般的卡尔曼滤波算法需要状态方程和测量方程均为线性方程。为了解决这一问题，引入EKF算法来实现INS/GPS松组合模型，步骤如下：
-
状态方程线性化并离散化。
首先对函数 f ( ) f() f()求偏导（该部分可以用matlab自动完成，详见本文附件中help_formula_derivation_ins_euler_directEKF_method.m文件），得到
F = ∂ f / x \bold{F}={\partial{f}}/{\bold{x}} F=∂f/x
根据公式(https://en.wikipedia.org/wiki/Discretization)
A = [ − F Q 0 F ⊤ ] T \mathbf{A}=\left[\begin{array}{cc} {-\mathbf{F}} & {\mathbf{Q}} \\ {\mathbf{0}} & {\mathbf{F}^{\top}} \end{array}\right] T A=[−F0QF⊤]T
及
e A = [ ⋯ F d − 1 Q d 0 F d ⊤ ] e^{\mathbf{A}}=\left[\begin{array}{cc} {\cdots} & {\mathbf{F}_{d}^{-1} \mathbf{Q}_{d}} \\ {\mathbf{0}} & {\mathbf{F}_{d}^{\top}} \end{array}\right] eA=[⋯0Fd−1QdFd⊤]
将 F d \mathbf{F}_{d} Fd记为 Φ k / k − 1 \bold{\Phi}_{k/k-1} Φk/k−1， Q d \mathbf{Q}_{d} Qd记为 Q k \mathbf{Q}_{k} Qk（ k k k表示当前时刻），可以将线性及离散化后的状态方程写为
x k / k − 1 = Φ k / k − 1 x k − 1 + Q k \bold{x}_{k/k-1}=\bold{\Phi}_{k/k-1}\bold{x}_{k-1}+\mathbf{Q}_{k} xk/k−1=Φk/k−1xk−1+Qk -
测量方程离散化.
由于测量方程本身为线性，不需要线性化步骤，可以直接改写为离散化形式
z k = H x k + R k \bold{z}_{k}=\bold{H}\bold{x}_{k}+\bold{R}_{k} zk=Hxk+Rk
其中，
H = ∂ h / x \bold{H} = {\partial{h}}/{\bold{x}} H=∂h/x
显然，这里的 H \bold{H} H中只包含0和1。
需要注意的是，由于航向角和位置速度测量量更新频率不同，磁强计的更新频率可达到上百赫兹，而GPS模块的更新频率只有5到10赫兹，所以实际工程实现中，往往需要多个测量矩阵，如 H m a g \bold{H}_{mag} Hmag、 H g p s \bold{H}_{gps} Hgps。 -
套用卡尔曼滤波五条公式进行迭代估计，按照实际的代码顺序的话就是
（1）状态一步预测：
x ^ k / k − 1 = Φ k / k − 1 x ^ k − 1 \hat{\bold{x}}_{k/k-1}=\bold{\Phi}_{k/k-1}\hat{\bold{x}}_{k-1} x^k/k−1=Φk/k−1x^k−1
（2）一步预测均方误差：
P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Q k − 1 \bold{P}_{k/k-1}=\bold{\Phi}_{k/k-1}\bold{P}_{k-1}\bold{\Phi}_{k/k-1}^T+\mathbf{Q}_{k-1} Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Qk−1
（3）测量值融合权重计算：
K k = Φ k / k − 1 H k T ( H k Φ k / k − 1 H k T + R k ) − 1 \bold{K}_k=\bold{\Phi}_{k/k-1}\bold{H}_k^T(\bold{H}_k\bold{\Phi}_{k/k-1}\bold{H}_k^T+\bold{R}_{k})^{-1} Kk=Φk/k−1HkT(HkΦk/k−1HkT+Rk)−1
（4）融合测量值：
x ^ k = x ^ k / k − 1 + K k ( z k − H k x k / k − 1 ) \hat{\bold{x}}_k=\hat{\bold{x}}_{k/k-1}+\bold{K}_k(\bold{z}_k-\bold{H}_k\bold{x}_{k/k-1}) x^k=x^k/k−1+Kk(zk−Hkxk/k−1)
（5）估计均方误差：
P k = ( I − K k H k ) P k / k − 1 \bold{P}_k=(\bold{I}-\bold{K}_k\bold{H}_k)\bold{P}_{k/k-1} Pk=(I−KkHk)Pk/k−1
Matlab实现状态估计的滤波过程代码如下（需要完整运行程序的可以下载本文附件）：
clc
clear
% 生成仿真数据
generate_uav_sensors_structure;
% 重力加速度常量，m/s^2
g_mps2 = 9.81;
% 定义directEKF算法相关变量
ekf = [];
% 磁强计和GPS测量噪声估计，用于构造 R 矩阵
ekf.sigmas.mag2D_meas_rad = sqrt(uavSensors.sigmas.mag2D_yaw_noise_rad^2);
ekf.sigmas.pos_meas_m = sqrt(uavSensors.sigmas.GPSpos_noise_m^2);
ekf.sigmas.vel_meas_mps = sqrt(uavSensors.sigmas.GPSvel_noise_mps^2);
ekf.R_gps = diag([...
[1 1 1]*ekf.sigmas.pos_meas_m ... % North-East-Alt position measurement noise, m
[1 1 1]*ekf.sigmas.vel_meas_mps ... % NED velocity measurement noise, m/s
].^2);
ekf.R_gps = max(ekf.R_gps,(1e-3)^2*eye(size(ekf.R_gps)));
ekf.R_mag2d = ekf.sigmas.mag2D_meas_rad;
ekf.R_mag2d = max(ekf.R_mag2d,(1e-3)^2*eye(size(ekf.R_mag2d)));
% 陀螺仪和加速度计初始估计的不确定度，用于构造P矩阵（协方差矩阵）
ekf.sigmas.gyro_bias_rps = uavSensors.sigmas.gyro_bias_rps;
ekf.sigmas.accel_bias_mps2 = uavSensors.sigmas.accel_bias_mps2;
% EKF过程噪声估计，用于构造 Q 矩阵
% Q 矩阵需要根据经验调节。Q 值代表了我们对状态模型的置信程度
ekf.sigmas.attitude_process_noise_rad = 0.002;%0.002;% Euler angle process noise, rad
ekf.sigmas.pos_process_noise_m = 0.005;%0; % Position process noise, m
ekf.sigmas.vel_process_noise_mps = 0.001;%2; % Velocity process noise, m/s
ekf.sigmas.gyroBias_process_noise_rps = 1e-5;%1e-6; % Gyro bias process noise, rad/s
ekf.sigmas.accelBias_process_noise_mps2=1e-5;%1e-6; % Accel bias process noise, m/s^2
ekf.Q = diag([...
[1 1 1]*ekf.sigmas.attitude_process_noise_rad ... % Euler angle process noise, rad
[1 1 1]*ekf.sigmas.pos_process_noise_m ... % North-East-Alt position process noise, m
[1 1 1]*ekf.sigmas.vel_process_noise_mps ... % NED velocity process noise, m/s
[1 1 1]*ekf.sigmas.gyroBias_process_noise_rps ... % XYZ gyro bias process noise, rad/s
[1 1 1]*ekf.sigmas.accelBias_process_noise_mps2 ... % XYZ accel bias process noise, m/s^2
].^2);
ekf.Q = max(ekf.Q,(1e-3)^2*eye(size(ekf.Q)));
% 设定EKF状态向量初始值
phi = 0; % 横滚角初始值，rad
theta=0; % 俯仰角初始值，rad
psi = uavSensors.mag2D_yaw_deg(1)*pi/180; % 偏航角初始值, rad
euler_init = [ phi theta psi ];
pos_init = [ uavSensors.GPS_north_m(1) uavSensors.GPS_east_m(1) uavSensors.GPS_h_msl_m(1) ];
vel_init = uavSensors.GPS_v_ned_mps(1,:);
ekf.xhat = [ ...
euler_init ... % 1-3: 对 Euler angle (Roll, Pitch, Yaw) 状态进行初始化，rad
pos_init ... % 4-6: 对 North-East-Alt 位置状态进行初始化, m
vel_init ... % 7-9: 对 NED 速度状态初始化, m/s
[0 0 0] ... % 10-12: 对 XYZ 三轴陀螺仪的常值偏差进行初始化, rad/s
[0 0 0] ... % 13-15: 对 XYZ 三轴加速度计的长治偏差进行初始化, m/s^2
]';
clear psi theta phi euler_init pos_init vel_init
% 对初始的协方差矩阵（P矩阵）设定较大值，以便EKF算法在初始的几次滤波步骤中更多的相信测量方程
large_angle_uncertainty_rad = 30*pi/180;
large_pos_uncertainty_m = 100;
large_vel_uncertainty_mps = 10;
ekf.P = diag([...
[1 1 1]*large_angle_uncertainty_rad ... % init Euler angle (NED-to-body) uncertainties, rad
[1 1 1]*large_pos_uncertainty_m ... % init North-East-Alt position uncertainties, m
[1 1 1]*large_vel_uncertainty_mps ... % init NED velocity uncertainties, m/s
[1 1 1]*ekf.sigmas.gyro_bias_rps ... % init XYZ gyro bias uncertainties, rad/s
[1 1 1]*ekf.sigmas.accel_bias_mps2 ... % init XYZ accel bias uncertainties, m/s^2
].^2);
ekf.P = max(ekf.P,(1e-3)^2*eye(size(ekf.P))); % 为了避免gyro_bias_rps和accel_bias_mps2设定为0
clear large_angle_uncertainty_rad large_pos_uncertainty_m large_vel_uncertainty_mps
% 定义uavEst结构体并分配内存空间，用于存储估计的中间结果
uavEst.xhat = zeros(length(uavTruth.time_s),length(ekf.xhat));
uavEst.P = zeros(length(uavTruth.time_s),length(ekf.xhat));
for kTime = 1 : length(uavSensors.time_s)
% 计算两次滤波的时间间隔
time_s = uavSensors.time_s(kTime);
if kTime == 1
dt_s = uavSensors.time_s(2) - uavSensors.time_s(1);
else
dt_s = time_s - uavSensors.time_s(kTime-1);
end
% 获取滤波器所估计的状态量
phi=ekf.xhat(1); theta=ekf.xhat(2); psi=ekf.xhat(3); % Roll, Pitch, Yaw Euler angles, rad
Pn=ekf.xhat(4); Pe=ekf.xhat(5); Alt=ekf.xhat(6); % Position, North/East/Altitude, meters
Vn=ekf.xhat(7); Ve=ekf.xhat(8); Vd=ekf.xhat(9); % Velocity, North/East/Down, m/s
bwx=ekf.xhat(10); bwy=ekf.xhat(11); bwz=ekf.xhat(12); % Gyro biases, rad/s
bax=ekf.xhat(13); bay=ekf.xhat(14); baz=ekf.xhat(15); % Accelerometer biases, m/s^2
% IMU 更新
gyro_wb_rps = uavSensors.gyro_wb_rps(kTime,:)'; % 3x1 vector
accel_fb_mps2 = uavSensors.accel_fb_mps2(kTime,:)'; % 3x1 vector
wx = gyro_wb_rps(1); wy = gyro_wb_rps(2); wz = gyro_wb_rps(3); % rad/s
fx = accel_fb_mps2(1); fy = accel_fb_mps2(2); fz = accel_fb_mps2(3); % m/s^2
% 计算 xdot
C_bodyrate2eulerdot = [1 sin(phi)*tan(theta) cos(phi)*tan(theta); ...
0 cos(phi) -sin(phi) ; ...
0 sin(phi)*sec(theta) cos(phi)*sec(theta)];
C_ned2b = [cos(theta)*cos(psi) cos(theta)*sin(psi) -sin(theta); ...
sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) sin(phi)*cos(theta); ...
cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi) cos(phi)*cos(theta)];
C_b2ned=transpose(C_ned2b);
xdot = [ ...
C_bodyrate2eulerdot*([wx;wy;wz]-[bwx;bwy;bwz]); ... % Derivative of [roll; pitch; yaw]
[Vn; Ve; -Vd]; ... % Derivative of [Pn; Pe; Alt]
C_b2ned*([fx;fy;fz]-[bax;bay;baz])+[0;0;g_mps2]; ... % Derivative of [Vn; Ve; Vd]
[0;0;0]; ... % Derivative of [bwx; bwy; bwz]
[0;0;0]; ... % Derivative of [bax; bay; baz]
];
% 计算 F 矩阵
F = [ ...
[ sin(phi)*tan(theta)*(bwz - wz) - cos(phi)*tan(theta)*(bwy - wy), - cos(phi)*(bwz - wz)*(tan(theta)^2 + 1) - sin(phi)*(bwy - wy)*(tan(theta)^2 + 1), 0, 0, 0, 0, 0, 0, 0, -1, -sin(phi)*tan(theta), -cos(phi)*tan(theta), 0, 0, 0]
[ cos(phi)*(bwz - wz) + sin(phi)*(bwy - wy), 0, 0, 0, 0, 0, 0, 0, 0, 0, -cos(phi), sin(phi), 0, 0, 0]
[ (sin(phi)*(bwz - wz))/cos(theta) - (cos(phi)*(bwy - wy))/cos(theta), - (cos(phi)*sin(theta)*(bwz - wz))/cos(theta)^2 - (sin(phi)*sin(theta)*(bwy - wy))/cos(theta)^2, 0, 0, 0, 0, 0, 0, 0, 0, -sin(phi)/cos(theta), -cos(phi)/cos(theta), 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0]
[ - (sin(phi)*sin(psi) + cos(phi)*cos(psi)*sin(theta))*(bay - fy) - (cos(phi)*sin(psi) - cos(psi)*sin(phi)*sin(theta))*(baz - fz), cos(psi)*sin(theta)*(bax - fx) - cos(phi)*cos(psi)*cos(theta)*(baz - fz) - cos(psi)*cos(theta)*sin(phi)*(bay - fy), (cos(phi)*cos(psi) + sin(phi)*sin(psi)*sin(theta))*(bay - fy) - (cos(psi)*sin(phi) - cos(phi)*sin(psi)*sin(theta))*(baz - fz) + cos(theta)*sin(psi)*(bax - fx), 0, 0, 0, 0, 0, 0, 0, 0, 0, -cos(psi)*cos(theta), cos(phi)*sin(psi) - cos(psi)*sin(phi)*sin(theta), - sin(phi)*sin(psi) - cos(phi)*cos(psi)*sin(theta)]
[ (cos(psi)*sin(phi) - cos(phi)*sin(psi)*sin(theta))*(bay - fy) + (cos(phi)*cos(psi) + sin(phi)*sin(psi)*sin(theta))*(baz - fz), sin(psi)*sin(theta)*(bax - fx) - cos(phi)*cos(theta)*sin(psi)*(baz - fz) - cos(theta)*sin(phi)*sin(psi)*(bay - fy), (cos(phi)*sin(psi) - cos(psi)*sin(phi)*sin(theta))*(bay - fy) - (sin(phi)*sin(psi) + cos(phi)*cos(psi)*sin(theta))*(baz - fz) - cos(psi)*cos(theta)*(bax - fx), 0, 0, 0, 0, 0, 0, 0, 0, 0, -cos(theta)*sin(psi), - cos(phi)*cos(psi) - sin(phi)*sin(psi)*sin(theta), cos(psi)*sin(phi) - cos(phi)*sin(psi)*sin(theta)]
[ cos(theta)*sin(phi)*(baz - fz) - cos(phi)*cos(theta)*(bay - fy), cos(theta)*(bax - fx) + cos(phi)*sin(theta)*(baz - fz) + sin(phi)*sin(theta)*(bay - fy), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, sin(theta), -cos(theta)*sin(phi), -cos(phi)*cos(theta)]
[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
];
% 计算 PHI_k & Q_k 矩阵（一步预测矩阵 和 离散形式的过程噪声矩阵）
nStates = length(ekf.xhat);
AA = [-F ekf.Q; zeros(nStates,nStates) F']*dt_s;
BB = expm(AA); % <- Matrix exponential!
PHI_k = BB(nStates+1:2*nStates,nStates+1:2*nStates)';
Q_k = PHI_k*BB(1:nStates,nStates+1:2*nStates);
% 一步预测 & 更新协方差
ekf.xhat = ekf.xhat + xdot*dt_s;
ekf.P = PHI_k*ekf.P*PHI_k' + Q_k;
% 磁强计测量融合（更新频率与IMU一致）
mag2D_yaw_rad = uavSensors.mag2D_yaw_deg(kTime)*pi/180;
ekf.H_mag2d = [ 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
ekf.K_mag2d = (ekf.P*ekf.H_mag2d')/(ekf.H_mag2d*ekf.P*ekf.H_mag2d'+ekf.R_mag2d);
% 消除yaw角的范围约束
while mag2D_yaw_rad>ekf.xhat(3)+pi, mag2D_yaw_rad=mag2D_yaw_rad-2*pi; end
while mag2D_yaw_rad<ekf.xhat(3)-pi, mag2D_yaw_rad=mag2D_yaw_rad+2*pi; end
ekf.Z_mag2d = mag2D_yaw_rad;
ekf.xhat = ekf.xhat + ekf.K_mag2d*(ekf.Z_mag2d - ekf.H_mag2d*ekf.xhat);
ekf.P = (eye(length(ekf.xhat))-ekf.K_mag2d*ekf.H_mag2d)*ekf.P;
% GPS 测量更新 （更新频率较低）
GPS_east_m = uavSensors.GPS_east_m(kTime);
GPS_north_m = uavSensors.GPS_north_m(kTime);
GPS_h_msl_m = uavSensors.GPS_h_msl_m(kTime);
GPS_v_ned_mps = uavSensors.GPS_v_ned_mps(kTime,:)'; % 3x1 vector
GPS_valid = uavSensors.GPS_valid(kTime);
if GPS_valid
ekf.H_gps = [ ...
[ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]
[ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
];
ekf.K_gps = (ekf.P*ekf.H_gps')/(ekf.H_gps*ekf.P*ekf.H_gps'+ekf.R_gps);
ekf.Z_gps = [ ...
[GPS_north_m; GPS_east_m; GPS_h_msl_m];
GPS_v_ned_mps
];
ekf.xhat = ekf.xhat + ekf.K_gps*(ekf.Z_gps - ekf.H_gps*ekf.xhat);
ekf.P = (eye(length(ekf.xhat))-ekf.K_gps*ekf.H_gps)*ekf.P;
end
%
uavEst.xhat(kTime,:) = ekf.xhat';
end
uavEst.time_s = uavSensors.time_s;
uavEst.north_m = uavEst.xhat(:,4);
uavEst.east_m = uavEst.xhat(:,5);
uavEst.h_msl_m = uavEst.xhat(:,6);
uavEst.v_ned_mps = uavEst.xhat(:,7:9);
uavEst.roll_deg = rad2deg(uavEst.xhat(:,1));
uavEst.pitch_deg = rad2deg(uavEst.xhat(:,2));
uavEst.yaw_deg = rad2deg(uavEst.xhat(:,3));
% 估计结果可视化
figure(1)
clf;
% 绘图的线型和点型设置
truthLineType = 'b-';
sensedLineType = 'gd';
estimateLineType = 'r.';
markerSize = 6;
linewidth = 2;
% 北-东方向的位置估计结果
subplot(4,2,[1 3])
plot(uavSensors.GPS_east_m, uavSensors.GPS_north_m, sensedLineType, ...
uavEst.east_m, uavEst.north_m, estimateLineType, ...
uavTruth.east_m, uavTruth.north_m, truthLineType, ...
'markersize',markerSize, 'linewidth', linewidth);
hold on
plot(uavTruth.east_m(1),uavTruth.north_m(1),'co', ...
'markersize',1.5*markerSize, 'linewidth', 2*linewidth);
hold off
axis equal
grid on
xlabel('East, m'); ylabel('North, m')
title('UAV Position')
legend('GPS Meas.','Est. Pos.','True Pos.','Start','location','best')
% 海拔高度估计结果
subplot(4,2,5)
plot(uavSensors.time_s, uavSensors.GPS_h_msl_m, sensedLineType, ...
uavEst.time_s, uavEst.h_msl_m, estimateLineType, ...
uavTruth.time_s, uavTruth.h_msl_m, truthLineType, ...
'markersize',markerSize, 'linewidth', linewidth);
grid on
xlabel('Time, s'); ylabel('Altitude, above MeanSeaLevel, m')
legend('GPS Meas.','Est. Alt','True Alt','location','best')
% 北-东-地 速度估计结果
subplot(4,2,7)
mag = @(v)(sqrt(sum(v.^2,2))); % magnitude of each row
plot(uavSensors.time_s, mag(uavSensors.GPS_v_ned_mps), sensedLineType, ...
uavEst.time_s, mag(uavEst.v_ned_mps), estimateLineType, ...
uavTruth.time_s, mag(uavTruth.v_ned_mps), truthLineType, ...
'markersize',markerSize, 'linewidth', linewidth);
grid on
xlabel('Time, s'); ylabel('Inertial Speed, m/s')
legend('GPS Meas.','Est. Speed','True Speed','location','best')
% 横滚角估计结果
subplot(3,2,2);
plot(uavEst.time_s, uavEst.roll_deg, estimateLineType, ...
uavTruth.time_s, uavTruth.roll_deg, truthLineType, ...
'markersize',markerSize, 'linewidth', linewidth);
grid on
xlabel('Time, s'); ylabel('Roll Angle, deg')
title('UAV Attitude: Roll')
legend('Est. Roll','True Roll','location','best')
% 俯仰角估计结果
subplot(3,2,4);
plot(uavEst.time_s, uavEst.pitch_deg, estimateLineType, ...
uavTruth.time_s, uavTruth.pitch_deg, truthLineType, ...
'markersize',markerSize, 'linewidth', linewidth);
grid on
xlabel('Time, s'); ylabel('Pitch Angle, deg')
title('UAV Attitude: Pitch')
legend('Est. Pitch','True Pitch','location','best')
% 偏航角估计结果（-180,180）
subplot(3,2,6);
wrap = @(angleDeg)(mod(angleDeg+180,360)-180); % Wrap from -180deg to 180deg.
plot(uavSensors.time_s, wrap(uavSensors.mag2D_yaw_deg), sensedLineType, ...
uavEst.time_s, wrap(uavEst.yaw_deg), estimateLineType, ...
uavTruth.time_s, wrap(uavTruth.yaw_deg), truthLineType, ...
'markersize',markerSize, 'linewidth', linewidth);
grid on
xlabel('Time, s'); ylabel('Yaw Angle, deg')
title('UAV Attitude: Yaw')
legend('Magnetometer','Est. Yaw','True Yaw','location','best')
本文代码在github及gitee（国内镜像）中不定时维护，代码内容可能与本文有所出入，地址：
https://github.com/tsuibeyond/BasicEKF
https://gitee.com/tsuibeyond/BasicEKF
估计结果如下图所示：
本文方法依然存在的问题，仍需继续讨论：
- EKF的状态量中，特别是IMU的测量偏差没有相应的测量值直接测量，算法是否能保证他们的可观性？因此有必要对算法进行可观性分析。
- EKF算法通过对非线性方程线性化处理，在原理本身上损失了一定的精度，可以尝试使用其他非线性滤波算法，来对比状态估计的效果。
- 在卡尔曼滤波五条公式中，融合测量值的过程中，涉及到了三个欧拉角直接加减法。欧拉角三个一组，表示载体的姿态角，不是标量，只有在小角度假设的前提下才可以当做标量处理加减法。是否能对方程进行改进，来绕过这个瑕疵。
1419
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
