How to use MATLAB's inertial navigation extended Kalman filter (insEKF) for pose estimation with accel and gyro data as inputs?

21 次查看（过去 30 天）
Effesian
Effesian 2022-9-12
评论： Marek Coufal 2024-3-19
Hello,
I want to estimate pose (position and orientation) from accel and gyro data. For this, I have used the insfilterMARG and called, in a loop, for each new accel and gyro sample pair the predict function: predict(insFilter,accelReadings,gyroReadings). This works reasonably well, but the insfilterMARG state vector contains a number of states I don't need (essentially all magnetometer related states). More importantly, as far as I can see, the insfilterMARG provides only a correct function that allows state correction with direct state measurements: correct(FUSE,idx,measurement,measurementCovariance). The insEKF by contrast, provides a fuse() function that would allow me to fuse signals that are not direct state measurements. However, I have a hard time understanding how to use insEKF. So far, I initialized the insEKF as shown in the code snippet below and in a loop I call the predict and fuse methods
accelSensor = insAccelerometer;
gyroSensor = insGyroscope;
motionModel = insMotionPose;
insFilter = insEKF(accelSensor, gyroSensor, motionModel);
for i=1:N
predict(insFilter,Ts); % Ts sample time
fuse(insFilter, accelSensor, accelReadings, someMeasurementNoiseAccel);
fuse(insFilter, gyroSensor, gyroReadings, someMeasurementNoiseGyro);
end
Finally, with stateparts I read out the state components I am interested in. However, the estimated position trajectory (and orientation) do not match at all the insfilterMARG (nor the ground truth shape).
What is the function call sequence to obtain comparable results with the insEKF as with the insfilterMARG?
Thanks!
  1 个评论
Marek Coufal
Marek Coufal 2024-3-19
Hello,
did you manage to get any acceptable outcome using the insEKF function? I am having pretty simillar problem using insEKF for fusing gyro, accel and magnetometer data from ADIS16505 (midrange 6DoF IMU) and LSM303 magnetometer. The trajectory estimates do not match the ground truth at all, and are two orders of magnitude bigger (altough I checked, that I am using correctly scaled data, i.e acceleration of not moving object in z direction is 9.81ms-2.
Thanks.

请先登录，再进行评论。

请先登录，再回答此问题。

回答（1 个）

Brian Fanous
Brian Fanous 2022-9-14
编辑：Brian Fanous 2022-9-14
Generally speaking, if you have only typical MEMs gyroscope and accelerometer you will not be able to get a high quality position estimate. The bias in these sensors, even after calibration, will adversely affect your position estimate. For a high quality position estimate you'll need to add another sensor - like a GPS.
If you only want orientation and have a gyroscope and accelerometer, the imufilter is a lot easier to start off with. Alternatively, you can estimate just orientation and angular velocity accurately with an insEKF by using the insMotionOrientation motion model instead of the insMotionPose motion model.
  3 个评论
显示 1更早的评论
Effesian
Effesian 2022-9-17
By the way, in the MathWorks documentation for the insMotionPose, the alogirthm section states: "The insMotionPose object models the orientation-only motion of platforms." --> That does not seem correct, since pose is both position and orientation.
Brian Fanous
Brian Fanous 2022-9-19
I will try to get the documentation for insMotionPose updated but in short....
insMotionPose is for orientation and position
insMotionOrientation is for orientation only.
Yes, the insEKF is intended to be a configurable inertial navigation filter. It is a continuous-discrete EKF. The insfilterMARG is a discrete-discrete EKF so they underlying filter is slightly different.

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 Inertial Sensor Fusion 的更多信息

标签

产品


版本

R2022a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by