APM飞控的EKF滤波程序代码（21状态量）解析

最新推荐文章于 2024-09-18 16:14:10 发布

Robert--cao

最新推荐文章于 2024-09-18 16:14:10 发布

阅读量2.8k

点赞数 1

本文链接：https://blog.csdn.net/caokaifa/article/details/81078256

版权

首先，给出21个状态量，这个状态量前面的一篇博客也给出了，这里在说明一下：

State vector:
% quaternions (q0, q1, q2, q3)
% Velocity - m/sec (North, East, Down)
% Position - m (North, East, Down)
% Delta Angle bias - rad (X,Y,Z)
% Wind Vector - m/sec (North,East)
% Earth Magnetic Field Vector - milligauss (North, East, Down)
% Body Magnetic Field Vector - milligauss (X,Y,Z)

%% define symbolic variables and constants 变量与常数定义
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 gn ge gd real % NED gravity vector - m/sec^2
syms omn ome omd real; % Earth rotation vector in local NED axes rad/sec - rad/sec
syms daxCov dayCov dazCov dvxCov dvyCov dvzCov 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

1.四元数更新

% define the measured Delta angle and delta velocity vectors 定义增量角与增量角速度
da = [dax; day; daz]; %x,y,z轴增量角
dv = [dvx; dvy; dvz];%x,y,z轴的zeng增量角速度

% define the delta angle and delta velocity bias errors
da_b = [dax_b; day_b; daz_b]; %定义增量角偏差

% define the quaternion rotation vector
quat = [q0;q1;q2;q3]; %定义四元数

% define the bias corrected delta angle
% Ignore coning compensation and earths rotation as these effect are
% negligible in terms of covariance growth compared to other efects for our
% grade of sensor
% deltaAngle = da - da_b + 1/12*cross(da_prev,da) - transpose(Cbn)*([omn; ome; omd])*dt;
deltaAngle = da - da_b; %校正后的增量角

delQuat = [1;
0.5*deltaAngle(1);
0.5*deltaAngle(2);
0.5*deltaAngle(3);
]; %校正增量角转化为等同的四元数

%更新后四元数qNew

qNew=[quat(1)*delQuat(1)-quat(2:4)'*delQuat(2:4);quat(1)*delQuat(2:4)+delQuat(1)*quat(2:4)+cross(quat(2:4),delQuat(2:4))];

2.速度更新

q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);

Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ...
2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ...
2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];

vNew = [vn;ve;vd] + [gn;ge;gd]*dt + Tbn*dv

3.位置更新

% define the position update equations
pNew = [pn;pe;pd] + [vn;ve;vd]*dt;

4.其他的变量直接赋值

% define the IMU bias error update equations
dabNew = [dax_b; day_b; daz_b];

% 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 process equations output vector
processEqns = [qNew;vNew;pNew;dabNew;vwnNew;vweNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew];

5.计算F

F = jacobian(processEqns, stateVector);%processEqns等式方程对变量stateVector中变量求导可得:

F[0][0] = 1;
F[0][1] = SF[6];
F[0][2] = SF[8];
F[0][3] = SF[7];
F[0][10] = SF[9];
F[0][11] = SF[10];
F[0][12] = SF[11];
F[1][0] = SF[5];
F[1][1] = 1;
F[1][2] = SF[4];
F[1][3] = SF[8];
F[1][10] = -q0/2;
F[1][11] = SF[11];
F[1][12] = -SF[10];
F[2][0] = SF[3];
F[2][1] = SF[7];
F[2][2] = 1;
F[2][3] = SF[5];
F[2][10] = -SF[11];
F[2][11] = -q0/2;
F[2][12] = SF[9];
F[3][0] = SF[4];
F[3][1] = SF[3];
F[3][2] = SF[6];
F[3][3] = 1;
F[3][10] = SF[10];
F[3][11] = -SF[9];
F[3][12] = -q0/2;
F[4][0] = SF[2];
F[4][1] = SF[0];
F[4][2] = SF[12] + SF[13] - 2*dvx*q2;
F[4][3] = 2*dvz*q1 - 2*dvy*q0 - 2*dvx*q3;
F[4][4] = 1;
F[5][0] = SF[1];
F[5][1] = 2*dvx*q2 - SF[13] - SF[12];
F[5][2] = SF[0];
F[5][3] = SF[2];
F[5][5] = 1;
F[6][0] = SF[12] + SF[13] - 2*dvx*q2;
F[6][1] = SF[1];
F[6][2] = 2*dvy*q3 - 2*dvx*q0 - 2*dvz*q2;
F[6][3] = SF[0];
F[6][6] = 1;
F[7][4] = dt;
F[7][7] = 1;
F[8][5] = dt;
F[8][8] = 1;
F[9][6] = dt;
F[9][9] = 1;
F[10][10] = 1;
F[11][11] = 1;
F[12][12] = 1;
F[13][13] = 1;
F[14][14] = 1;
F[15][15] = 1;
F[16][16] = 1;
F[17][17] = 1;
F[18][18] = 1;
F[19][19] = 1;
F[20][20] = 1;
其中：
SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3;
SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
SF[3] = day/2 - day_b/2;
SF[4] = daz/2 - daz_b/2;
SF[5] = dax/2 - dax_b/2;
SF[6] = dax_b/2 - dax/2;
SF[7] = daz_b/2 - daz/2;
SF[8] = day_b/2 - day/2;
SF[9] = q1/2;
SF[10] = q2/2;
SF[11] = q3/2;
SF[12] = 2*dvz*q0;
SF[13] = 2*dvy*q1;