insAccelerometer Documentation h(x)

Question

Lorenz 2025-11-19，14:08

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2181370-insaccelerometer-documentation-h-x

回答： Paul 2025-11-21，22:26

I am currently working on building an Extended Kalman Filter with Accelerometer measurements and looked into the implementation of the h(x) measurement equation.

In the documentation and in the file this equation is, when estimating the acceleration in the state, h(x)=g_sensor+a_sensor+bias which is equal to h(x)=bias+R_sensor->navigation*(g_navigation+a_navigation). However, in the actual implementation of the measurement(sensor, filt) function h(x) appears to be calculated by h(x)=bias-R_sensor->navigation*(a_navigation - g_navigation).

Does anynone know why the actual implementation seems to be different to the documented equation?

Thanks in advance!

4 个评论
显示 2更早的评论隐藏 2更早的评论

William Rose 2025-11-21，6:11

在 MATLAB Online 中打开

@Lorenz,

When I click the link you provided, I get redirected to this address. I assume that is because I am in the USA. The web page includes the following (screenshot)

The equation which I have labelled "eq.1" matches what you posted, where you use "bias" and the web page uses "Δ".

You say that eq.1 is equal to

h(x)=bias+R_sensor->navigation*(g_navigation+a_navigation) eq.2

I do not understand eq.2 as you have written it. It looks like C++ code for accessing parts of a structure. Matlab would use

h(x)=bias+R_sensor.navigation*(g_navigation+a_navigation) eq.3

which would equal eq.1, if R_sensor.navigation equals 1. Am I mis-understanding something here?

You say:

the actual implementation of the measurement(sensor, filt) function h(x) appears to be calculated by

h(x)=bias-R_sensor->navigation*(a_navigation - g_navigation) eq.4

and you say

I read the implementation in function z = measurement(sensor, filt)

Please help me find the code that has eq.4. The only code I can find which contains "z=measurement(sensor,filt)" is the example file ...MATLAB\Examples\R2024a\shared_positioning\CustomizeSensorModelUsedWithInsEKFExample\BiasSensor.m. This file includes the following, which does not have an equation similar to eq.4:

classdef BiasSensor < positioning.INSSensorModel
    %BIASSENSOR Sensor measuring velocity with bias
    %   Copyright 2021 The MathWorks, Inc.    
    methods 
        % some lines deleted
        function z = measurement(sensor,filter)
            % Measurement is the summation of the velocity measurement and
            % the bias.
            velocity = stateparts(filter,'Velocity');
            bias = stateparts(filter,sensor,'Bias');
            z = velocity + bias;
        end
        % some lines deleted
    end
end

If you help others locate the code you are talking about, maybe someone on this site will be able to assist.

Lorenz 2025-11-21，7:00

在 MATLAB Online 中打开

I found eq.4 in .../MATLAB_R2025a.app/toolbox/shared/insframework/core/insframework/insAccelerometer.m

        function z = measurement(sensor, filt)
            grav = getGravity(sensor, filt);
            orient = stateparts(filt, 'Orientation');
            bias = stateparts(filt, sensor, 'Bias');
            biasBodyX = bias(1);
            biasBodyY = bias(2);
            biasBodyZ = bias(3);
            gravityNavX = grav(1);
            gravityNavY = grav(2);
            gravityNavZ = grav(3);
            q0 = orient(1);
            q1 = orient(2);
            q2 = orient(3);
            q3 = orient(4);
            z = zeros(1,3, 'like', filt.State);
            
            if isConstVel(sensor,filt) 
                z(1) = biasBodyX + gravityNavX*(2*q0^2 + 2*q1^2 - 1) + gravityNavY*(2*q0*q3 + 2*q1*q2) - gravityNavZ*(2*q0*q2 - 2*q1*q3);
                z(2) = biasBodyY + gravityNavY*(2*q0^2 + 2*q2^2 - 1) - gravityNavX*(2*q0*q3 - 2*q1*q2) + gravityNavZ*(2*q0*q1 + 2*q2*q3);
                z(3) = biasBodyZ + gravityNavZ*(2*q0^2 + 2*q3^2 - 1) + gravityNavX*(2*q0*q2 + 2*q1*q3) - gravityNavY*(2*q0*q1 - 2*q2*q3);
            else
                % Non-zero linear acceleration present in accelerometer
                % signal
                acceleration = stateparts(filt, 'Acceleration');     
                accelNavX = acceleration(1);
                accelNavY = acceleration(2);
                accelNavZ = acceleration(3);
                z(1) = biasBodyX - (accelNavX - gravityNavX)*(2*q0^2 + 2*q1^2 - 1) - (accelNavY - gravityNavY)*(2*q0*q3 + 2*q1*q2) + (accelNavZ - gravityNavZ)*(2*q0*q2 - 2*q1*q3);
                z(2) = biasBodyY - (accelNavY - gravityNavY)*(2*q0^2 + 2*q2^2 - 1) + (accelNavX - gravityNavX)*(2*q0*q3 - 2*q1*q2) - (accelNavZ - gravityNavZ)*(2*q0*q1 + 2*q2*q3);
                z(3) = biasBodyZ - (accelNavZ - gravityNavZ)*(2*q0^2 + 2*q3^2 - 1) - (accelNavX - gravityNavX)*(2*q0*q2 + 2*q1*q3) + (accelNavY - gravityNavY)*(2*q0*q1 - 2*q2*q3);
            end
        end

in the else case of the if clause the z (my case with acceleration estimated in state) is calculated with eq.4.

请先登录，再进行评论。

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

Answer 1

Paul 2025-11-21，22:26

1
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2181370-insaccelerometer-documentation-h-x#answer_1572199

在 MATLAB Online 中打开

My take on this ...

The equations in the code make sense assuming (1) the accelNav vector, which I assume (2) is a filter state estimate, is the inertial acceleration of the object. I assume (3) the accelerometer measurement input to the insEKF should be specific force, so the predicted measurement, z, should subtract the gravity vector from the state estimate, and then resolve the difference into the body-fixed coordinate frame.

One test would be to assume the body is in free fall while aligned with the insEKF reference frame and see what the insEKF estimates for acceleration.

accel = insAccelerometer;

gyro = insGyroscope;

filt = insEKF(accel,gyro,insMotionPose)

filt =

insEKF with properties: State: [22×1 double] StateCovariance: [22×22 double] AdditiveProcessNoise: [22×22 double] MotionModel: [1×1 insMotionPose] Sensors: {[1×1 insAccelerometer] [1×1 insGyroscope]} SensorNames: {'Accelerometer' 'Gyroscope'} ReferenceFrame: 'NED'

filt.stateinfo

ans = struct with fields:

Orientation: [1 2 3 4] AngularVelocity: [5 6 7] Position: [8 9 10] Velocity: [11 12 13] Acceleration: [14 15 16] Accelerometer_Bias: [17 18 19] Gyroscope_Bias: [20 21 22]

filt.AdditiveProcessNoise = blkdiag(zeros(13),10*eye(3),zeros(6));

dt = 0.01; N = 10;

for ii = 1:N

if ii ~= 1

predict(filt,dt);

end

fuse(filt,accel,[0,0,0],1e-4);

accest(ii,:) = stateparts(filt,"Acceleration");

end

figure

plot((0:N-1)*dt,accest,'-o'),yline(9.81)

The insEKF estimates 9.8 m/sec^2 in the +Z direction of the reference frame, which is Down in this case, which is the inertial acceleration of the body in free fall.

This type of approach can be repeated with different orientations of the body to verify that the orientation state defines the orientation of the body frame relative to the reference frame and to verify that the acceleration state estimate is, in fact, resolved in the reference frame (not the body frame, which would make no sense at all IMO).

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

insAccelerometer Documentation h(x)

4 个评论
显示 2更早的评论隐藏 2更早的评论

回答（1 个）

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

insAccelerometer Documentation h(x)

4 个评论 显示 2更早的评论隐藏 2更早的评论

回答（1 个）

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

4 个评论
显示 2更早的评论隐藏 2更早的评论

0 个评论
显示 -2更早的评论隐藏 -2更早的评论