PIX4源码EKF的MATLAB代码实现

% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
%这个脚本需要MATLAB符号工具箱运行三个小时！！！

% Derivation of Navigation EKF using a local NED earth Tangent Frame and
% XYZ body fixed frame
% Sequential fusion of velocity and position measurements
% Fusion of true airspeed
% Sequential fusion of magnetic flux measurements
% 24 state architecture.
% IMU data is assumed to arrive at a constant rate with a time step of dt
% IMU delta angle and velocity data are used as control inputs,
% not observations

% Author:  Paul Riseborough

% State vector:状态向量
% attitude quaternion
% Velocity - m/sec (North, East, Down)
% Position - m (North, East, Down)
% Delta Angle bias - rad (X,Y,Z)
% Delta Velocity bias - m/s (X,Y,Z)
% Earth Magnetic Field Vector - (North, East, Down)
% Body Magnetic Field Vector - (X,Y,Z)
% Wind Vector  - m/sec (North,East)

% Observations:观测量
% NED velocity - m/s
% NED position - m
% True airspeed - m/s
% angle of sideslip - rad
% XYZ magnetic flux

% Time varying parameters:
% XYZ delta angle measurements in body axes - rad
% XYZ delta velocity measurements in body axes - m/sec

%% define symbolic variables and constants定义符号变量和常量
clear all;
reset(symengine);
syms dax day daz real % IMU delta angle measurements in body axes - rad
syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
syms vn ve vd real % NED velocity - m/sec
syms pn pe pd real % NED position - m
syms dax_b day_b daz_b real % delta angle bias - rad
syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec
syms dt real % IMU time step - sec
syms gravity real % gravity  - m/sec^2重力加速度
syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances
syms vwn vwe real; % NE wind velocity - m/sec风速
syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss机体磁场
syms magN magE magD real; % NED earth fixed magnetic field components - milligauss地磁场
syms R_VN R_VE R_VD real % variances for NED velocity measurements - (m/sec)^2大地系中速度的方差
syms R_PN R_PE R_PD real % variances for NED position measurements - m^2大地系中位置测量的方差
syms R_TAS real  % variance for true airspeed measurement - (m/sec)^2真空速测量的方差
syms R_MAG real  % variance for magnetic flux measurements - milligauss^2
syms R_BETA real % variance of sidelsip measurements rad^2侧滑角测量的方差
syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
syms ptd real % location of terrain in D axis
syms decl real; % earth magnetic field declination from true north磁偏角
syms R_DECL R_YAW real; % variance of declination or yaw angle observation磁偏角和偏航角观测的方差
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y  body axes在x轴和y轴上的风相对运动的弹道系数的倒数
syms rho real % air density (kg/m^3)大气密度
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2加速度测量的方差
syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s)真空速沿各轴的X和Y体比力wrt分量的导数

%% define the state prediction equations定义状态预测方程

% define the measured Delta angle and delta velocity vectors
dAngMeas = [dax; day; daz];
dVelMeas = [dvx; dvy; dvz];

% define the IMU bias errors and scale factor
dAngBias = [dax_b; day_b; daz_b];
dVelBias = [dvx_b; dvy_b; dvz_b];

% define the quaternion rotation vector for the state estimate
quat = [q0;q1;q2;q3];
% derive the truth body to nav direction cosine matrix推导机体系到导航坐标系的余弦矩阵
Tbn = Quat2Tbn(quat);

% define the truth delta angle定义真实角度增量，忽略圆锥振动补偿
% ignore coning compensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
dAngTruth = dAngMeas - dAngBias;

% Define the truth delta velocity -ignore sculling and transport rate
% corrections as these negligible are in terms of covariance growth for our
% application and grade of sensor
dVelTruth = dVelMeas - dVelBias;

% define the attitude update equations定义姿态更新方程
% use a first order expansion of rotation to calculate the quaternion increment
% acceptable for propagation of covariances
deltaQuat = [1;
0.5*dAngTruth(1);
0.5*dAngTruth(2);
0.5*dAngTruth(3);
];
quatNew = QuatMult(quat,deltaQuat);

% define the velocity update equations定义速度更新方程
% ignore coriolis terms for linearisation purposes因为线性化的目的忽略科里奥利力
vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;

% define the position update equations定义位置更新方程
pNew = [pn;pe;pd] + [vn;ve;vd]*dt;

% define the IMU error update equations定义IMU误差更新方程
dAngBiasNew = dAngBias;
dVelBiasNew = dVelBias;

% define the wind velocity update equations定义风速更新方程
vwnNew = vwn;
vweNew = vwe;

% define the earth magnetic field update equations定义地磁场更新方程
magNnew = magN;
magEnew = magE;
magDnew = magD;

% define the body magnetic field update equations定义机体磁场更新方程
magXnew = magX;
magYnew = magY;
magZnew = magZ;

% Define the state vector & number of states定义状态矢量和状态个数
stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe];
nStates=numel(stateVector);

% Define vector of process equations定义状态方程矢量
newStateVector = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];

% derive the state transition matrix推导状态转移矩阵
F = jacobian(newStateVector, stateVector);
% set the rotation error states to zero
[F,SF]=OptimiseAlgebra(F,'SF');

% define a symbolic covariance matrix using strings to represent 定义符号协方差矩阵使用字符串表示
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:nStates
for colIndex = 1:nStates
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
end
end

save 'StatePrediction.mat';

%% derive the covariance prediction equations推导协方差矩阵
% This reduces the number of floating point operations by a factor of 6 or这使得浮点运算的数量减少了6倍或更多，比起代码中使用标准的矩阵运算。
% more compared to using the standard matrix operations in code

% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
% velocities, after bias effects have been removed.
%惯性解决方案中的误差增长被认为是由角度和速度的增量“噪声”所驱动的，在偏差影响被消除之后。

% derive the control(disturbance) influence matrix from IMu noise to state从IMU噪声到状态的控制(扰动)影响矩阵
% noise
G = jacobian(newStateVector, [dAngMeas;dVelMeas]);
[G,SG]=OptimiseAlgebra(G,'SG');

% derive the state error matrix推导出状态误差矩阵
distMatrix = diag([daxVar dayVar dazVar dvxVar dvyVar dvzVar]);
Q = G*distMatrix*transpose(G);
[Q,SQ]=OptimiseAlgebra(Q,'SQ');

% Derive the predicted covariance matrix using the standard equation利用标准方程推导出预测的协方差矩阵
PP = F*P*transpose(F) + Q;

% Collect common expressions to optimise processing收集常用表达式来优化处理
[PP,SPP]=OptimiseAlgebra(PP,'SPP');

save('StateAndCovariancePrediction.mat');%状态和协方差矩阵预测
clear all;
reset(symengine);

%% derive equations for fusion of true airspeed measurements推导真航速融合的方程
load('StatePrediction.mat');
VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian观测矩阵雅克比
[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing优化处理
K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);%求解卡尔曼增益，R_TAS真空速测量的方差
[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector卡尔曼增益矢量

% save equations and reset workspace保存方程重置工作空间
save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS');
clear all;
reset(symengine);

%% derive equations for fusion of angle of sideslip measurements推导侧滑角测量的融合方程
load('StatePrediction.mat');

% calculate wind relative velocities in nav frame and rotate into body frame在导航坐标系中推导相对风速转换到机体坐标系
Vbw = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd];
% calculate predicted angle of sideslip using small angle assumption利用小角度假设，计算侧滑角的预测角？
BetaPred = Vbw(2)/Vbw(1);
H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian
[H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector

% save equations and reset workspace
save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA');
clear all;
reset(symengine);

%% derive equations for fusion of magnetic field measurement推导磁场融合的方程
load('StatePrediction.mat');

magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian计算雅克比矩阵
[H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG');

%分开计算磁力计三轴卡尔曼增益是因为磁力计融合模式的不同
K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector
[K_MX,SK_MX]=OptimiseAlgebra(K_MX,'SK_MX');
K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); % Kalman gain vector
[K_MY,SK_MY]=OptimiseAlgebra(K_MY,'SK_MY');
K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector
[K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');

% save equations and reset workspace
save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ');
clear all;
reset(symengine);

%% derive equations for sequential fusion of optical flow measurements推导光流融合方程
load('StatePrediction.mat');

% Range is defined as distance from camera focal point to object measured
% along sensor Z axis距离定义为从摄像机焦点到测量沿传感器Z轴的物体的距离
syms range real;

% Define rotation matrix from body to sensor frame定义从机体到传感器的旋转矩阵
syms Tbs_a_x Tbs_a_y Tbs_a_z real;
syms Tbs_b_x Tbs_b_y Tbs_b_z real;
syms Tbs_c_x Tbs_c_y Tbs_c_z real;
Tbs = [ ...
Tbs_a_x Tbs_a_y Tbs_a_z ; ...
Tbs_b_x Tbs_b_y Tbs_b_z ; ...
Tbs_c_x Tbs_c_y Tbs_c_z ...
];

% Calculate earth relative velocity in a non-rotating sensor frame在非旋转的传感器框架中计算地球相对速度
relVelSensor = Tbs * transpose(Tbn) * [vn;ve;vd];

% Divide by range to get predicted angular LOS rates relative to X and Y
% axes. Note these are rates in a non-rotating sensor frame
%除以距离来得到预测的角速度相对于X轴和Y轴。注意这些是在一个非旋转的传感器框架的速率
losRateSensorX = +relVelSensor(2)/range;
losRateSensorY = -relVelSensor(1)/range;

save('temp1.mat','losRateSensorX','losRateSensorY');

clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');

% calculate the observation Jacobian and Kalman gain for the X axis计算X轴观测雅克比矩阵和卡尔曼系数
H_LOSX = jacobian(losRateSensorX,stateVector); % measurement Jacobian
H_LOSX = simplify(H_LOSX);
K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector
K_LOSX = simplify(K_LOSX);
save('temp2.mat','H_LOSX','K_LOSX');
ccode([H_LOSX;transpose(K_LOSX)],'file','LOSX.c');
fix_c_code('LOSX.c');

clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');

% calculate the observation Jacobian for the Y axis计算Y轴的观测雅克比矩阵
H_LOSY = jacobian(losRateSensorY,stateVector); % measurement Jacobian
H_LOSY = simplify(H_LOSY);
K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector卡尔曼增益矢量
K_LOSY = simplify(K_LOSY);
save('temp3.mat','H_LOSY','K_LOSY');
ccode([H_LOSY;transpose(K_LOSY)],'file','LOSY.c');
fix_c_code('LOSY.c');

% reset workspace
clear all;
reset(symengine);

%% derive equations for sequential fusion of body frame velocity measurements推导机体轴系速度的融合
load('StatePrediction.mat');

% body frame velocity observations
syms velX velY velZ real;

% velocity observation variance
syms R_VEL real;

% calculate relative velocity in body frame计算机体系中的相对速度（视觉里程计）
relVelBody = transpose(Tbn)*[vn;ve;vd];

save('temp1.mat','relVelBody','R_VEL');

% calculate the observation Jacobian for the X axis计算X轴观测雅克比矩阵
H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian
H_VELX = simplify(H_VELX);
save('temp2.mat','H_VELX');
ccode(H_VELX,'file','H_VELX.c');
fix_c_code('H_VELX.c');

clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');

% calculate the observation Jacobian for the Y axis计算Y轴速度的观测雅克比矩阵
H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian
H_VELY = simplify(H_VELY);
save('temp3.mat','H_VELY');
ccode(H_VELY,'file','H_VELY.c');
fix_c_code('H_VELY.c');

clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');

% calculate the observation Jacobian for the Z axis计算Z轴速度观测的雅克比矩阵
H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian
H_VELZ = simplify(H_VELZ);
save('temp4.mat','H_VELZ');
ccode(H_VELZ,'file','H_VELZ.c');
fix_c_code('H_VELZ.c');

clear all;
reset(symengine);

% calculate Kalman gain vector for the X axis计算卡尔曼增益
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');

K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
K_VELX = simplify(K_VELX);
ccode(K_VELX,'file','K_VELX.c');
fix_c_code('K_VELX.c');

clear all;
reset(symengine);

% calculate Kalman gain vector for the Y axis
load('StatePrediction.mat');
load('temp1.mat');
load('temp3.mat');

K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
K_VELY = simplify(K_VELY);
ccode(K_VELY,'file','K_VELY.c');
fix_c_code('K_VELY.c');

clear all;
reset(symengine);

% calculate Kalman gain vector for the Z axis
load('StatePrediction.mat');
load('temp1.mat');
load('temp4.mat');

K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
K_VELZ = simplify(K_VELZ);
ccode(K_VELZ,'file','K_VELZ.c');
fix_c_code('K_VELZ.c');

% reset workspace
clear all;
reset(symengine);

% calculate Kalman gains vectors for X,Y,Z to take advantage of common
% terms
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');
load('temp3.mat');
load('temp4.mat');
K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
K_VEL = simplify([K_VELX,K_VELY,K_VELZ]);%化简方程式
ccode(K_VEL,'file','K_VEL.c');
fix_c_code('K_VEL.c');

%% derive equations for fusion of 321 sequence yaw measurement推导欧拉角321序列偏航角测量的融合方程
load('StatePrediction.mat');

% Calculate the yaw (first rotation) angle from the 321 rotation sequence计算偏航角测量角度
angMeas = atan(Tbn(2,1)/Tbn(1,1));
H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian
H_YAW321 = simplify(H_YAW321);
ccode(H_YAW321,'file','calcH_YAW321.c');
fix_c_code('calcH_YAW321.c');

% reset workspace
clear all;
reset(symengine);

%% derive equations for fusion of 312 sequence yaw measurement推导欧拉角312序列偏航角测量角度
load('StatePrediction.mat');

% Calculate the yaw (first rotation) angle from an Euler 312 sequence
angMeas = atan(-Tbn(1,2)/Tbn(2,2));
H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea
H_YAW312 = simplify(H_YAW312);
ccode(H_YAW312,'file','calcH_YAW312.c');
fix_c_code('calcH_YAW312.c');

% reset workspace
clear all;
reset(symengine);

%% derive equations for fusion of declination推导磁偏角融合方程
load('StatePrediction.mat');

% the predicted measurement is the angle wrt magnetic north of the horizontal
% component of the measured field
angMeas = atan(magE/magN);%计算磁偏角
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian观测雅克比矩阵
H_MAGD = simplify(H_MAGD);
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
K_MAGD = simplify(K_MAGD);
ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c');
fix_c_code('calcMAGD.c');

% reset workspace
clear all;
reset(symengine);

%% derive equations for fusion of lateral body acceleration (multirotors only)对于多旋翼推导了横向加速度的融合方程
load('StatePrediction.mat');

% use relationship between airspeed along the X and Y body axis and the
% drag to predict the lateral acceleration for a multirotor vehicle type
% where propulsion forces are generated primarily along the Z body axis
%利用X轴和Y轴的飞行速度和阻力来预测多旋翼飞行器的侧向加速度，在此情况下，推进力主要沿Z轴方向产生
vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity

% calculate drag assuming flight along axis in positive direction
% sign change will be looked after in implementation rather than by adding
% sign functions to symbolic derivation which genererates output with dirac
% functions
%计算阻力沿轴向正方向的变化将在执行过程中进行，而不是通过在符号推导中增加符号函数，从而使输出具有狄拉克函数delta函数
% accXpred = -0.5*rho*vrel(1)*vrel(1)*BCXinv; % predicted acceleration measured along X body axis
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis

% Use a simple viscous drag model for the linear estimator equations
% Use the the derivative from speed to acceleration averaged across the
% speed range对于线性估计方程使用简单的粘性阻力模型
%使用在速度范围内平均的从速度到加速度的导数
% The nonlinear equation will be used to calculate the predicted
% measurement in implementation
accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis
accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis

% Derive observation Jacobian and Kalman gain matrix for X accel fusion
H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
H_ACCX = simplify(H_ACCX);
[H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing
K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
[K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector

% Derive observation Jacobian and Kalman gain matrix for Y accel fusion
H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
H_ACCY = simplify(H_ACCY);
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector

% save equations and reset workspace
save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY');
clear all;
reset(symengine);

%% Save output and convert to m and c code fragments保存输出并转换成m和c代码片段

% load equations for predictions and updates
load('StateAndCovariancePrediction.mat');
load('Airspeed.mat');
load('Sideslip.mat');
load('Magnetometer.mat');
load('Drag.mat');

fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName);
SaveScriptCode(nStates);
ConvertToM(nStates);
ConvertToC(nStates);
© 2018 GitHub, Inc.