package
Estimation.StrapdownINS.UKF
Manifold unscented Kalman filter for aided strapdown navigation
Use
Estimator implements the shared sampled strapdown interface. It carries the same nominal position, velocity, quaternion and IMU biases as the ESKF, with 31 sigma points in a 15-dimensional tangent. The quaternion is not treated as four independent Euclidean error coordinates.
Use the same sensor captures, covariance, terrain assumptions and initial uncertainty when comparing filters. More sigma points do not by themselves establish better consistency or robustness. Inspect prediction/correction acceptance and runtime cost along with RMS error.
The nominal vector has 16 entries; its local covariance has 15 by 15 entries. Do not interchange those layouts. Shared frames and timing are documented in PartialEstimator.
Public declarations
Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.
| Role | Name | Type | Dimensions / attributes / default | Description |
|---|---|---|---|---|
| constant | TangentLength | Integer | = 15 | |
| constant | SigmaCount | Integer | = 31 | 2*n + 1 state sigma points |
| constant | SigmaScale | Real | = 3.872983346207417 | sqrt(TangentLength), for alpha=1, beta=2, kappa=0 |
| constant | SigmaWeight | Real | = 0.03333333333333333 | 1/(2*TangentLength) for every noncentral sigma point |
| constant | CentralCovarianceWeight | Real | = 2.0 | w0 covariance weight for alpha=1, beta=2, kappa=0 |
Members
| Name | Kind | Description |
|---|---|---|
| TangentVector | type | |
| Covariance | type | |
| NominalVector | type | |
| State | record | Nominal strapdown state and local 15-state covariance |
| lowerCholesky | function | Fixed-size lower Cholesky factor with an affirmative success result |
| sigmaTangents | function | Generate 2*n+1 symmetric sigma points in the local tangent |
| stateVector | function | Flatten a public UKF state into its nominal numeric representation |
| injectVector | function | Right-inject a local tangent into a flattened nominal state |
| localErrorVector | function | Body-local tangent carrying one flattened nominal state to another |
| predictNominalVector | function | Propagate a flattened nominal state through the strapdown mechanization |
| predictPreintegratedNominalVector | function | Apply a shared IMU preintegral to one UKF sigma-state bias hypothesis |
| localError | function | Body-local tangent carrying reference to target |
| outerProduct | function | Fixed-size tangent outer product |
| initialize | function | |
| predict | function | Unscented propagation through the strapdown nominal mechanization |
| predictPreintegrated | function | Unscented propagation through one closed-form IMU preintegration packet |
| correctUnscented | function | Unscented measurement correction in local navigation coordinates |
| correctGps | function | Unscented joint GPS position/velocity correction |
| correctMagnetometer | function | Unscented yaw-only correction from raw magnetic field |
| correctBarometer | function | Unscented vertical-position correction from bias-compensated pressure altitude |
| correctOpticalFlow | function | Unscented body-velocity correction from co-timed flow and range |
| correctMocap | function | Unscented motion-capture pose correction |
| step | function | Execute one sampled UKF prediction/correction tick |
| Estimator | block | Sampled manifold UKF implementing the common strapdown estimator boundary |
Modelica declarations and implementation
Show parameters, interfaces and source
Parameters, inputs, outputs, defaults and equations from the source file.
package UKF
"Manifold unscented Kalman filter for aided strapdown navigation"
constant Integer TangentLength = 15;
constant Integer SigmaCount = 31 "2*n + 1 state sigma points";
constant Real SigmaScale = 3.872983346207417
"sqrt(TangentLength), for alpha=1, beta=2, kappa=0";
constant Real SigmaWeight = 0.03333333333333333
"1/(2*TangentLength) for every noncentral sigma point";
constant Real CentralCovarianceWeight = 2.0
"w0 covariance weight for alpha=1, beta=2, kappa=0";
type TangentVector = Real[15]
"{position, velocity, attitude, gyro bias, accelerometer bias}";
type Covariance = Real[15, 15];
type NominalVector = Real[16]
"{position(3),velocity(3),quaternion(4),gyro bias(3),accel bias(3)}";
annotation(Documentation(info = "<html>
<h4>Use</h4>
<p><a href=\"modelica://Estimation.StrapdownINS.UKF.Estimator\">Estimator</a>
implements the shared sampled strapdown interface. It carries the same nominal
position, velocity, quaternion and IMU biases as the ESKF, with 31 sigma points
in a 15-dimensional tangent. The quaternion is not treated as four independent
Euclidean error coordinates.</p>
<p>Use the same sensor captures, covariance, terrain assumptions and initial
uncertainty when comparing filters. More sigma points do not by themselves
establish better consistency or robustness. Inspect prediction/correction
acceptance and runtime cost along with RMS error.</p>
<p>The nominal vector has 16 entries; its local covariance has 15 by 15 entries.
Do not interchange those layouts. Shared frames and timing are documented in
<a href=\"modelica://Estimation.StrapdownINS.PartialEstimator\">PartialEstimator</a>.</p>
</html>"));
end UKF;