correct function on insFilter insfilterNonholonomic

2 次查看（过去 30 天）
Marvin Dongus
Marvin Dongus 2022-12-5
回答： Ryan Salvo 2022-12-7
Im using the insfilterNonholonomic Object from Matlab. I want to correct the orientation of my state to the correct yaw-angel thats given from the GPS measurements. If im using the correct funtion on my filter the filter have a unpredictable behavior. I shortened my project to a correct function that get the State and the covariance an set it to the state. Therefor i think the correct function shouldn't have any effect, but it does.
Am i write that a correction of the current state with the current state an its correspondig covariance does not have any effect?
Why is my filter instable at certan time intervals?
Do i use the correct function correct?
My code(shortened):
gndFusion = insfilterNonholonomic('ReferenceFrame', 'NED', 'IMUSampleRate', imuFs,'ReferenceLocation', localOrigin, 'DecimationFactor', 2);
...
%init of filter
...
for i = 1:1:sim_rounds
predict(gndFusion, [accel_x(sampleIdx) accel_y(sampleIdx) accel_z(sampleIdx)], [gyro_x(sampleIdx) gyro_y(sampleIdx) gyro_z(sampleIdx)]);
if speed(sampleIdx) > 10
correct(gndFusion,1:4,gndFusion.State(1:4),gndFusion.StateCovariance(1:4)); % correct function that souldn't have any effect
end
end
with correct function:
without correct function:

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

回答（1 个）

Ryan Salvo
Ryan Salvo 2022-12-7
Ran in:
Hi Marvin,
Using the code snippet from your question, the call to the correct object function will have an effect on the filter state covariance. You can check this by saving the intial state and state covariance values before the call to correct.
filt = insfilterNonholonomic;
initState = filt.State;
initStateCov = filt.StateCovariance;
idx = 1:4;
correct(filt, idx, filt.State(idx), filt.StateCovariance(idx,idx));
initState(idx)
ans = 4×1
1 0 0 0
filt.State(idx)
ans = 4×1
1 0 0 0
initStateCov(idx,idx)
ans = 4×4
1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1
filt.StateCovariance(idx,idx)
ans = 4×4
0.5000 0 0 0 0 0.5000 0 0 0 0 0.5000 0 0 0 0 0.5000
Thanks,
Ryan

类别

在 Help Center 和 File Exchange 中查找有关 Audio Processing Algorithm Design 的更多信息

标签

产品


版本

R2022b

Community Treasure Hunt

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

Start Hunting!

Translated by