How to correctly obtain the signs of the measurement function h(x) from the kinematics of non holonomic filter?
2 次查看(过去 30 天)
显示 更早的评论
In the script "NHConstrainedIMUGPSFuserBase" from the Sensor Fusion and Tracking Toolbox, there is the function named "measurementFcnKinematics."
function h = measurementFcnKinematics(~, x)
%MEASUREMENTFCNKINEMATICS Measurement function h(x) for state vector x
% 2 measurements from kinematic constraints
% [velY, velZ];
q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);
gbX = x(5); %#ok<NASGU>
gbY = x(6); %#ok<NASGU>
gbZ = x(7); %#ok<NASGU>
pn = x(8); %#ok<NASGU>
pe = x(9); %#ok<NASGU>
pd = x(10); %#ok<NASGU>
vn = x(11);
ve = x(12);
vd = x(13);
abX = x(14); %#ok<NASGU>
abY = x(15); %#ok<NASGU>
abZ = x(16); %#ok<NASGU>
h = ...
[
q0*(q1*vd + q0*ve - q3*vn) + q1*(q0*vd - q1*ve + q2*vn) + q2*(q3*vd + q2*ve + q1*vn) - q3*(q3*ve - q2*vd + q0*vn);
q0*(q0*vd - q1*ve + q2*vn) - q1*(q1*vd + q0*ve - q3*vn) + q2*(q3*ve - q2*vd + q0*vn) + q3*(q3*vd + q2*ve + q1*vn);
];
end
The equations of h are described on the referenced bibliography of the insfilternonholonomic [1] Munguía, R. "A GPS-Aided Inertial Navigation System in Direct Configuration." Journal of applied research and technology. Vol. 12, Number 4, 2014, pp. 803 – 814. as follows:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1580491/image.png)
Where the rotation matrix is:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1580496/image.png)
I have been trying to understand the signs of the equations in MATLAB, because when I calculate them by hand, the result I obtain is as follows:
h = ...
[
q0*(-q1*vd + q0*ve + q3*vn) + q1*(-q0*vd - q1*ve + q2*vn) + q2*(q3*vd + q2*ve + q1*vn) + q3*(-q3*ve + q2*vd + q0*vn);
q0*(q0*vd + q1*ve - q2*vn) - q1*(q1*vd - q0*ve - q3*vn) + q2*(q3*ve - q2*vd - q0*vn) + q3*(q3*vd + q2*ve + q1*vn);
];
So, in summary, I would like to know if anyone else has noticed this difference or if they are able to obtain the same result as in MATLAB using the equations from the mentioned paper.
Thank you very much in advance.
0 个评论
采纳的回答
Brian Fanous
2024-1-5
The quaternion to rotation matrix conversion equation you've posted is for an active rotation (what the quatenrion class calls a "point" rotation).
The convention in these insfilters is to use passive (or "frame") rotations, regardless of what the original paper does. So the conversion is different.
0 个评论
更多回答(0 个)
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Unit Conversions 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!