New filter for distance sensor validation #24106
Draft
+282
−98
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 1376 byte (0.07 %)]px4_fmu-v6x [Total VM Diff: 1368 byte (0.07 %)]Updated: 2024-12-13T13:28:06 |
bresch
requested changes
Dec 13, 2024
| z = sf.Symbol("z") | ||
| dist_bottom = sf.Symbol("dist_bottom") | ||
| terrain = z - dist_bottom | ||
| state = sf.V2.symbolic("state") |
There was a problem hiding this comment.
You can define the states in a Values structure like here:
You can also pass a Values structure as an argument of jacobian
|
|
||
| updateConsistency(vz, time_us); | ||
| _x(RangeFilter::z.idx) += dt * vz; | ||
| _P = _A * _P * _A.transpose() + Q; |
There was a problem hiding this comment.
_A is just the identity matrix, right? So you could just do _P += Q and not store _A as a member variable
There was a problem hiding this comment.
Or even directly apply the noise to P the diagonal elements of P instead of first setting them in Q fillowed by a matrix addition. e.g.: _P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) = terrain_process_noise;
| // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2 | ||
| const float var = 2.f * dist_bottom_var / (dt * dt); | ||
| _innov_var = var + vz_var; | ||
| _R(RangeFilter::z.idx, RangeFilter::z.idx) = z_var; |
There was a problem hiding this comment.
Probably no need to store _R as a member variable is you anyways override it at every iteration
| float _gate{1.f}; | ||
| int _sample_count{0}; | ||
| KinematicState _state{KinematicState::UNKNOWN}; | ||
| int _min_nr_of_samples{0}; |
There was a problem hiding this comment.
instead of computing how many samples are required and counting the number of samples you could simply accumulate the dt until it reaches the desired period (e.g.> 1s in your case).
| _P = 0.5f * (_P + _P.transpose()); // Ensure symmetry | ||
| _innov = y(RangeFilter::terrain.idx); | ||
| _innov_var = S(RangeFilter::terrain.idx, RangeFilter::terrain.idx); | ||
| _test_ratio_lpf.update(test_ratio); |
There was a problem hiding this comment.
you should really use a "signed test raio lpf" here as the test ratio is always positive and the average of the test ratio of a gaussian white noise is >0 while the average of the signed test ratio would be 0
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Solved Problem
The distance sensor is being fused or used to reset the terrain state, even though its measurements do not accurately represent the distance to the ground due to obstructions caused by rain, snow, fog etc.
Solution
Now the distance sensor can be in 3 different states based on the lpf test ratio of a simple kalman filter:
consistent,inconsistentandunknown. A terrain reset can only happen when the state is consistent. The sensor will be used for normal fusion in both consistent and inconsistent states. Together with the already existing fog detector, the robustness against sensor blockage/failure is increased.Screenshots will follow