insfilterAsync

估计异步MARG和GPS数据的姿势

展开所有页面

Description

TheinsfilterAsync对象实现MARG和GPS数据的传感器融合，以估计NED（或ENU）参考帧的姿势。MARG（磁性，角速率，重力）数据通常分别来自磁力计，陀螺和加速度计数据。过滤器使用28元元状态向量来跟踪方向quaternion, velocity, position, MARG sensor biases, and geomagnetic vector. TheinsfilterAsyncobject uses a continuous-discrete extended Kalman filter to estimate these quantities.

创建

Syntax

filter = insfilterAsync

filter = insfilterasync（“ReferenceFrame”,RF)

filter = insfilterasync（___,Name,Value)

Description

example

filter= insfilterasync.creates aninsfilterAsyncobject to fuse asynchronous MARG and GPS data with default property values.

filter= insfilterasync.(“ReferenceFrame”,RF.)allows you to specify the reference frame,RF., of thefilter. SpecifyRF.as'NED'(North-East-Down) or'ENU'（东北北）。默认值是'NED'.

filter= insfilterasync.(___,Name,Value)also allows you set properties of the createdfilterusing one or more name-value pairs. Enclose each property name in single quotes.

特性

展开全部

`ReferenceLocation`—Reference location (deg, deg, meters)
`[0 0 0]`（默认）|three-element positive row vector

Reference location, specified as a three-element row vector in geodetic coordinates (latitude, longitude, and altitude). Altitude is the height above the reference ellipsoid model, WGS84. The reference location units are [degrees degrees meters].

数据类型：单身的|双倍的

`Quatternionnoise.`—添加性四元管过程噪声方差
`[1E-6 1E-6 1E-6 1E-6]`（默认）|标量|four-element row vector

添加性四元管过程噪声方差，指定为标量或四元素矢量的四元数零件。

数据类型：单身的|双倍的

`AngularVelocityNoise`—Additive angular velocity process noise in local navigation coordinate system ((rad/s)²)
`[0.005 0.005 0.005]`（默认）|标量|three-element row vector

（RAD / S）中的局部导航坐标系中的附加角速度过程噪声², specified as a scalar or three-element row vector of positive real finite numbers.

IfAngularVelocityNoiseis a row vector, the elements correspond to the noise in thex,y，和zaxes of the local navigation coordinate system, respectively.
IfAngularVelocityNoise是标量，单个元素应用于每个轴。

数据类型：单身的|双倍的

`PositionNoise`—局部导航坐标系中的附加位置过程噪声方差（M²)
`[1e-6 1e-6 1e-6]`（默认）|标量|three-element row vector

在M中的局部导航坐标系中的附加位置过程噪声², specified as a scalar or three-element row vector of positive real finite numbers.

IfPositionNoiseis a row vector, the elements correspond to the noise in thex,y，和zaxes of the local navigation coordinate system, respectively.
IfPositionNoise是标量，单个元素应用于每个轴。

数据类型：单身的|双倍的

`Velocitynoise.`—局部导航坐标系中的附加速度过程噪声方差（（m / s）²)
`[1e-6 1e-6 1e-6]`（默认）|标量|three-element row vector

（M / S）中局部导航坐标系中的附加速度过程噪声², specified as a scalar or three-element row vector of positive real finite numbers.

IfVelocitynoise.is a row vector, the elements correspond to the noise in thex,y，和zaxes of the local navigation coordinate system, respectively.
IfVelocitynoise.是标量，单个元素应用于每个轴。

数据类型：单身的|双倍的

`AccelerationNoise`—Additive acceleration process noise variance in local navigation coordinate system ((m/s²)²)
`[50 50 50]`（默认）|标量|three-element row vector

Additive acceleration process noise in (m/s²)², specified as a scalar or three-element row vector of positive real finite numbers.

IfAccelerationNoiseis a row vector, the elements correspond to the noise in thex,y，和zaxes of the local navigation coordinate system, respectively.
IfAccelerationNoise是标量，单个元素应用于每个轴。

数据类型：单身的|双倍的

`GyroscopeBiasNoise`—Additive process noise variance from gyroscope bias ((rad/s)²)
`[1e-10 1e-10 1e-10]`（默认）|标量|three-element row vector

来自陀螺仪偏置的添加剂过程噪声方差（RAD / S）², specified as a scalar or three-element row vector of positive real finite numbers.

IfGyroscopeBiasNoiseis a row vector, the elements correspond to the noise in thex,y，和z分别陀螺轴。
IfGyroscopeBiasNoise是标量，单个元素应用于每个轴。

数据类型：单身的|双倍的

`加速度计`—Additive process noise variance from accelerometer bias ((m/s²)²)
`[1e-4 1e-4 1e-4]`（默认）|positive scalar|three-element row vector

加速度计偏置的添加剂过程噪声方差（M / S.²)²，指定为正实数的标量或三元素行向量。

If加速度计is a row vector, the elements correspond to the noise in thex,y，和z加速度计的轴分别。
If加速度计是标量，单个元素应用于每个轴。

`GeomagneticVectorNoise`—Additive process noise variance of geomagnetic vector in local navigation coordinate system (μT²)
`[1e-6 1e-6 1e-6]`（默认）|positive scalar|three-element row vector

Additive process noise variance of geomagnetic vector in μT²，指定为正实数的标量或三元素行向量。

IfGeomagneticVectorNoiseis a row vector, the elements correspond to the noise in thex,y，和zaxes of the local navigation coordinate system, respectively.
IfGeomagneticVectorNoise是标量，单个元素应用于每个轴。

`磁性仪纤维网`—磁力计偏置的添加剂过程噪声方差（μT²)
`[0.1 0.1 0.1]`（默认）|positive scalar|three-element row vector

μT磁力计偏压的添加剂过程噪声方差²，指定为正实数的标量或三元素行向量。

If磁性仪纤维网is a row vector, the elements correspond to the noise in thex,y，和zaxes of the magnetometer, respectively.
If磁性仪纤维网是标量，单个元素应用于每个轴。

`State`—State vector of extended Kalman filter
28-element column vector

State vector of the extended Kalman filter. The state values represent:

State	Units	Index
Orientation (quaternion parts)	N / A.	1：4
Angular Velocity (XYZ)	Rad / S.	5：7
位置（ned或enu）	m	8:10
速度（ned或enu）	m/s	11:13
Acceleration (NED or ENU)	m/s²	14:16
Accelerometer Bias (XYZ)	m/s²	17:19
Gyroscope Bias (XYZ)	Rad / S.	20:22
Geomagnetic Field Vector (NED or ENU)	μT	23:25
Magnetometer Bias (XYZ)	μT	26:28

The default initial state corresponds to an object at rest located at[0 0 0]in geodetic LLA coordinates.

数据类型：单身的|双倍的

`StateCovariance`—State error covariance for extended Kalman filter
`眼睛（28）`（默认）|28-by-28 matrix

扩展卡尔曼滤波器的状态错误协方差，指定为实数的28×28元元矩阵。

数据类型：单身的|双倍的

Object Functions

`预测`	基于运动模型更新状态`insfilterAsync`
`fuseaccel`	Correct states using accelerometer data for`insfilterAsync`
`fusegyro`	使用陀螺数据正确的状态`insfilterAsync`
`fusemag`	Correct states using magnetometer data for`insfilterAsync`
`fusegps`	Correct states using GPS data for`insfilterAsync`
`correct`	使用直接状态测量来正确的状态`insfilterAsync`
`residual`	Residuals and residual covariances from direct state measurements for`insfilterAsync`
`Resealcel.`	加速度计测量的残差和残余协方差`insfilterAsync`
`ResealGPS.`	Residuals and residual covariance from GPS measurements for`insfilterAsync`
`residualmag`	磁力计测量的残差和残余协方差`insfilterAsync`
`ResealGegro.`	陀螺仪测量的残余和残余协方差`insfilterAsync`
`pose`	Current position, orientation, and velocity estimate for`insfilterAsync`
`重置`	重置内部状态`insfilterAsync`
`州诺夫`	Display state vector information for`insfilterAsync`
`copy`	创建副本of`insfilterAsync`
`tune`	Tune`insfilterAsync`减少估计错误的参数
`tunernoise`	Noise structure of fusion filter

Examples

collapse all

Estimate Pose of UAV

打开直播脚本

Load logged sensor data and ground truth pose.

加载（'Uavshort.mat','RESTOC','instate','imufs',......'accel','gyro','mag','lla','gpsvel',......'trueOrient','truePos')

创建ins筛选器以保险为熔断器MARG和GPS数据来估计姿势。

filt = insfilterasync;filt.referenceLocation = RESTOC;filt.state = [inittate（1：4）; 0; 0; 0; initstate（5:10）; 0; 0; 0; initstate（11：结束）];

Define sensor measurement noises. The noises were determined from datasheets and experimentation.

Rmag = 80; Rvel = 0.0464; Racc = 800; Rgyro = 1e-4; Rpos = 34;

Preallocate variables for position and orientation. Allocate a variable for indexing into the GPS data.

n =大小（Accel，1）;p =零（n，3）;q =零（n，1，'quaternion'); gpsIdx = 1;

保险丝加速度计，陀螺仪，磁力计和GPS数据。外部循环在IMU采样率下预测过滤器前一步一步并熔化加速度计和陀螺数据。

forII = 1：n％预测过滤器前一步一步预测(filt,1./imuFs);％保险丝加速度计和陀螺仪读数Fuseaccel（Filt，Accel（II，:)，RACC）;Fusegyro（Filt，Gyro（II，:)，rgyro）;％熔丝磁力计在1/2的IMU速率下if~mod(ii, fix(imuFs/2)) fusemag(filt,mag(ii,:),Rmag);end％熔断器每秒一次if~mod(ii,imuFs) fusegps(filt,lla(gpsIdx,:),Rpos,gpsvel(gpsIdx,:),Rvel); gpsIdx = gpsIdx + 1;end% Log the current pose estimate[p（ii，:)，q（ii）] =姿势（filt）;end

Calculate the RMS errors between the known true position and orientation and the output from the asynchronous IMU filter.

poserr = truepos  -  p;qerr = rad2deg（dist（tryorient，q））;prms = sqrt（平均值（poserr. ^ 2））;qrms = sqrt（平均值（qerr. ^ 2））;FPRINTF（'Position RMS Error\n');

Position RMS Error

FPRINTF（'\ tx：％.2f，y：％.2f，z：％.2f（米）\ n \ n',pRMS(1),pRMS(2),pRMS(3));

X: 0.55, Y: 0.71, Z: 0.74 (meters)

FPRINTF（'四元距离rms错误\ n');

四元距离rms误差

FPRINTF（'\ t％.2f（度）\ n \ n'，qrms）;

4.72（度）

Visualize the true position and the estimated position.

plot3(truePos(:,1),truePos(:,2),truePos(:,3),'行宽',2) hold在Plot3（p（：，1），p（:,2），p（:,3），'r:','行宽'，2）网格在xlabel('n（m）'）ylabel（'e（m）') zlabel('d（m）')

Figure contains an axes object. The axes object contains 2 objects of type line.

Algorithms

展开全部

Dynamic Model Used in`insfilterAsync`

Note: The following algorithm only applies to an NED reference frame.

insfilterAsync使用顺序融合实现28轴连续离散扩展卡尔曼滤波器。过滤器依赖于假设各个传感器测量不相关。过滤器使用全向运动模型，并呈现恒定的角速度和恒定加速度。该州被定义为：

$x = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ a n g V e l_{X} \\ a n g V e l_{Y} \\ a n g V e l_{Z} \\ p o s i t i o n_{N} \\ p o s i t i o n_{E} \\ p o s i t i o n_{D} \\ ν_{N} \\ ν_{E} \\ ν_{D} \\ a c c e l_{N} \\ a c c e l_{E} \\ a c c e l_{D} \\ a c c e l b i a s_{X} \\ a c c e l b i a s_{Y} \\ a c c e l b i a s_{Z} \\ g y r o b i a s_{X} \\ g y r o b i a s_{Y} \\ g y r o b i a s_{Z} \\ g e o m a g n e t i c F i e l d V e c t o r_{N} \\ g e o m a g n e t i c F i e l d V e c t o r_{E} \\ g e o m a g n e t i c F i e l d V e c t o r_{D} \\ m a g b i a s_{X} \\ m a g b i a s_{Y} \\ m a g b i a s_{Z} \end{matrix}]$

在哪里

q₀,q₁,q₂,q₃–– Parts of orientation quaternion. The orientation quaternion represents a frame rotation from the platform's current orientation to the local NED coordinate system.
angVel_X,angVel_Y,angVel_Z–– Angular velocity relative to the platform's body frame.
位置_N,位置_E,位置_D–– Position of the platform in the local NED coordinate system.
ν_N,ν_E,ν_D–– Velocity of the platform in the local NED coordinate system.
accel_N,accel_E,accel_D–– Acceleration of the platform in the local NED coordinate system.
accelbias._X,accelbias._Y,accelbias._Z- 加速度计读数中的偏见。
gyrobias_X,gyrobias_Y,gyrobias_Z–– Bias in the gyroscope reading.
geomagneticFieldVector_N,geomagneticFieldVector_E,geomagneticFieldVector_D- 估计参考位置的地磁场向量。
magbias_X,magbias_Y,magbias_Z–– Bias in the magnetometer readings.

鉴于过程方程的常规形成， $\dot{x} = f (x) + w$ ,wis the process noise, $\dot{x}$ is the derivative ofx，和：

$f (x) = [\begin{matrix} \frac{- (q_{1}) (a n g V e l_{X}) - (q_{2}) (a n g V e l_{Y}) - (q_{3}) (a n g V e l_{Z})}{2} \\ \frac{(q_{0}) (a n g V e l_{X}) - (q_{3}) (a n g V e l_{Y}) + (q_{1}) (a n g V e l_{Z})}{2} \\ \frac{(q_{3}) (a n g V e l_{X}) + (q_{0}) (a n g V e l_{Y}) - (q_{1}) (a n g V e l_{Z})}{2} \\ \frac{(q_{1}) (a n g V e l_{X}) - (q_{2}) (a n g V e l_{Y}) + (q_{0}) (a n g V e l_{Z})}{2} \\ 0 \\ 0 \\ 0 \\ ν_{N} \\ ν_{E} \\ ν_{D} \\ a c c e l_{N} \\ a c c e l_{E} \\ a c c e l_{D} \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{matrix}]$

insfilterAsync

Description

创建

Syntax

Description

特性

`ReferenceLocation`—Reference location (deg, deg, meters)
`[0 0 0]`（默认）|three-element positive row vector

`Quatternionnoise.`—添加性四元管过程噪声方差
`[1E-6 1E-6 1E-6 1E-6]`（默认）|标量|four-element row vector

`AngularVelocityNoise`—Additive angular velocity process noise in local navigation coordinate system ((rad/s)²)
`[0.005 0.005 0.005]`（默认）|标量|three-element row vector

`PositionNoise`—局部导航坐标系中的附加位置过程噪声方差（M²)
`[1e-6 1e-6 1e-6]`（默认）|标量|three-element row vector

`Velocitynoise.`—局部导航坐标系中的附加速度过程噪声方差（（m / s）²)
`[1e-6 1e-6 1e-6]`（默认）|标量|three-element row vector

`AccelerationNoise`—Additive acceleration process noise variance in local navigation coordinate system ((m/s²)²)
`[50 50 50]`（默认）|标量|three-element row vector

`GyroscopeBiasNoise`—Additive process noise variance from gyroscope bias ((rad/s)²)
`[1e-10 1e-10 1e-10]`（默认）|标量|three-element row vector

`加速度计`—Additive process noise variance from accelerometer bias ((m/s²)²)
`[1e-4 1e-4 1e-4]`（默认）|positive scalar|three-element row vector

`GeomagneticVectorNoise`—Additive process noise variance of geomagnetic vector in local navigation coordinate system (μT²)
`[1e-6 1e-6 1e-6]`（默认）|positive scalar|three-element row vector

`磁性仪纤维网`—磁力计偏置的添加剂过程噪声方差（μT²)
`[0.1 0.1 0.1]`（默认）|positive scalar|three-element row vector

`State`—State vector of extended Kalman filter
28-element column vector

`StateCovariance`—State error covariance for extended Kalman filter
`眼睛（28）`（默认）|28-by-28 matrix

Object Functions

Examples

Estimate Pose of UAV

Algorithms

Dynamic Model Used in`insfilterAsync`

扩展能力

C / c++代码生成
Generate C and C++ code using MATLAB® Coder™.

See Also

Navigation Toolbox Documentation

万博1manbetx

insfilterAsync

Description

创建

Syntax

Description

特性

ReferenceLocation—Reference location (deg, deg, meters)[0 0 0]（默认）|three-element positive row vector

Quatternionnoise.—添加性四元管过程噪声方差[1E-6 1E-6 1E-6 1E-6]（默认）|标量|four-element row vector

AngularVelocityNoise—Additive angular velocity process noise in local navigation coordinate system ((rad/s)2)[0.005 0.005 0.005]（默认）|标量|three-element row vector

PositionNoise—局部导航坐标系中的附加位置过程噪声方差（M2)[1e-6 1e-6 1e-6]（默认）|标量|three-element row vector

Velocitynoise.—局部导航坐标系中的附加速度过程噪声方差（（m / s）2)[1e-6 1e-6 1e-6]（默认）|标量|three-element row vector

AccelerationNoise—Additive acceleration process noise variance in local navigation coordinate system ((m/s2)2)[50 50 50]（默认）|标量|three-element row vector

GyroscopeBiasNoise—Additive process noise variance from gyroscope bias ((rad/s)2)[1e-10 1e-10 1e-10]（默认）|标量|three-element row vector

加速度计—Additive process noise variance from accelerometer bias ((m/s2)2)[1e-4 1e-4 1e-4]（默认）|positive scalar|three-element row vector

GeomagneticVectorNoise—Additive process noise variance of geomagnetic vector in local navigation coordinate system (μT2)[1e-6 1e-6 1e-6]（默认）|positive scalar|three-element row vector

磁性仪纤维网—磁力计偏置的添加剂过程噪声方差（μT2)[0.1 0.1 0.1]（默认）|positive scalar|three-element row vector

State—State vector of extended Kalman filter28-element column vector

StateCovariance—State error covariance for extended Kalman filter眼睛（28）（默认）|28-by-28 matrix

Object Functions

Examples

Estimate Pose of UAV

Algorithms

Dynamic Model Used ininsfilterAsync

扩展能力

C / c++代码生成Generate C and C++ code using MATLAB® Coder™.

See Also

Navigation Toolbox Documentation

万博1manbetx

`ReferenceLocation`—Reference location (deg, deg, meters)
`[0 0 0]`（默认）|three-element positive row vector

`Quatternionnoise.`—添加性四元管过程噪声方差
`[1E-6 1E-6 1E-6 1E-6]`（默认）|标量|four-element row vector

`AngularVelocityNoise`—Additive angular velocity process noise in local navigation coordinate system ((rad/s)²)
`[0.005 0.005 0.005]`（默认）|标量|three-element row vector

`PositionNoise`—局部导航坐标系中的附加位置过程噪声方差（M²)
`[1e-6 1e-6 1e-6]`（默认）|标量|three-element row vector

`Velocitynoise.`—局部导航坐标系中的附加速度过程噪声方差（（m / s）²)
`[1e-6 1e-6 1e-6]`（默认）|标量|three-element row vector

`AccelerationNoise`—Additive acceleration process noise variance in local navigation coordinate system ((m/s²)²)
`[50 50 50]`（默认）|标量|three-element row vector

`GyroscopeBiasNoise`—Additive process noise variance from gyroscope bias ((rad/s)²)
`[1e-10 1e-10 1e-10]`（默认）|标量|three-element row vector

`加速度计`—Additive process noise variance from accelerometer bias ((m/s²)²)
`[1e-4 1e-4 1e-4]`（默认）|positive scalar|three-element row vector

`GeomagneticVectorNoise`—Additive process noise variance of geomagnetic vector in local navigation coordinate system (μT²)
`[1e-6 1e-6 1e-6]`（默认）|positive scalar|three-element row vector

`磁性仪纤维网`—磁力计偏置的添加剂过程噪声方差（μT²)
`[0.1 0.1 0.1]`（默认）|positive scalar|three-element row vector

`State`—State vector of extended Kalman filter
28-element column vector

`StateCovariance`—State error covariance for extended Kalman filter
`眼睛（28）`（默认）|28-by-28 matrix

Dynamic Model Used in`insfilterAsync`

C / c++代码生成
Generate C and C++ code using MATLAB® Coder™.