APM飛控的擴展kalman濾波21、22、23、24個狀態量

The filter estimates the following states: 4 quaternion parameters 3 North,East,Down velocity components 3 North,East,Down position components 3 IMU delta angle bias components 2 IMU delta velocity bias in X and Y (only included in 24 state derivation) 1 IMU delta velocity bias in Z (only included in the 22 and 23 state derivation) 2 North,East wind velocity components 3 North,East,Down earth magnetic flux components 3 X,Y,Z body fixed magnetic flux components (these are opposite sign to the compass offsets used by APM) 1 Offset of terrain along down axis (only included in the 23 state filter derivation)

% 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)

    [0 ... 3] 四元數
    [4 ... 6] 速度 NED (m/s)
    [7 ... 9] 位置 NED (m)
    [10 ... 12] IMU 角度增量偏差 XYZ (rad)
    [13 ... 15] IMU 速度增量偏差 XYZ (m/s)
    [16 ... 18] 地球磁場 NED (gauss)
    [19 ... 21] 機體磁場 XYZ (gauss)
    [22 ... 23] 風速 NE (m/s)

其實上面的話可以理解爲21與24個狀態的區別在於24個狀態包含了Delta Velocity bias - m/s (X,Y,Z)這個三個量，其它的都一樣，22個狀態是在21個狀態中增加了z(IMU delta velocity bias in Z),

23三個狀態就是在22個狀態中增加了Offset of terrain along down axis