|
| 1 | +/* __ _ __ __ _ _ |
| 2 | +| |/ / /\ | | | \/ | /\ | \ | | |
| 3 | +| ' / / \ | | | \ / | / \ | \| | |
| 4 | +| < / /\ \ | | | |\/| | / /\ \ | . ` | |
| 5 | +| . \ / ____ \| |____| | | |/ ____ \| |\ | |
| 6 | +|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| |
| 7 | +
|
| 8 | +Kalman Filter |
| 9 | +Version 0.4.0 |
| 10 | +https://github.com/FrancoisCarouge/Kalman |
| 11 | +
|
| 12 | +SPDX-License-Identifier: Unlicense |
| 13 | +
|
| 14 | +This is free and unencumbered software released into the public domain. |
| 15 | +
|
| 16 | +Anyone is free to copy, modify, publish, use, compile, sell, or |
| 17 | +distribute this software, either in source code form or as a compiled |
| 18 | +binary, for any purpose, commercial or non-commercial, and by any |
| 19 | +means. |
| 20 | +
|
| 21 | +In jurisdictions that recognize copyright laws, the author or authors |
| 22 | +of this software dedicate any and all copyright interest in the |
| 23 | +software to the public domain. We make this dedication for the benefit |
| 24 | +of the public at large and to the detriment of our heirs and |
| 25 | +successors. We intend this dedication to be an overt act of |
| 26 | +relinquishment in perpetuity of all present and future rights to this |
| 27 | +software under copyright law. |
| 28 | +
|
| 29 | +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
| 30 | +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF |
| 31 | +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. |
| 32 | +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR |
| 33 | +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, |
| 34 | +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
| 35 | +OTHER DEALINGS IN THE SOFTWARE. |
| 36 | +
|
| 37 | +For more information, please refer to <https://unlicense.org> */ |
| 38 | + |
| 39 | +#include "fcarouge/kalman.hpp" |
| 40 | +#include "fcarouge/quantity_linalg.hpp" |
| 41 | +#include "fcarouge/unit.hpp" |
| 42 | + |
| 43 | +#include <cassert> |
| 44 | +#include <format> |
| 45 | + |
| 46 | +namespace fcarouge::test { |
| 47 | +namespace { |
| 48 | +using state = |
| 49 | + fcarouge::state<quantity_vector<m, m / s, m / s2, m, m / s, m / s2>>; |
| 50 | +using output_t = quantity_vector<m, m>; |
| 51 | +using estimate_uncertainty = |
| 52 | + fcarouge::estimate_uncertainty<ᴀʙᵀ<state::type, state::type>>; |
| 53 | +using process_uncertainty = |
| 54 | + fcarouge::process_uncertainty<ᴀʙᵀ<state::type, state::type>>; |
| 55 | +using output_uncertainty = |
| 56 | + fcarouge::output_uncertainty<ᴀʙᵀ<output_t, output_t>>; |
| 57 | +using output_model = fcarouge::output_model<ᴀʙᵀ<output_t, state::type>>; |
| 58 | +using state_transition = |
| 59 | + fcarouge::state_transition<ᴀʙᵀ<state::type, state::type>>; |
| 60 | + |
| 61 | +template <auto... Reference> |
| 62 | +inline fcarouge::output_t<quantity_vector<Reference...>> output{ |
| 63 | + fcarouge::output<quantity_vector<Reference...>>}; |
| 64 | + |
| 65 | +//! @test Verifies compatibility with the `mp-units` quantities and units |
| 66 | +//! library for C++ in the case of an algebraic 6x2x0 filter estimating the |
| 67 | +//! vehicle location. |
| 68 | +//! |
| 69 | +//! @details See the sample for details. |
| 70 | +[[maybe_unused]] auto test{[] { |
| 71 | + kalman filter{ |
| 72 | + state{0. * m, 0. * m / s, 0. * m / s2, 0. * m, 0. * m / s, 0. * m / s2}, |
| 73 | + output<m, m>, |
| 74 | + estimate_uncertainty{500. * identity<estimate_uncertainty::type>}, |
| 75 | + process_uncertainty{[]() { |
| 76 | + process_uncertainty::type value{identity<process_uncertainty::type>}; |
| 77 | + value[0, 0] = 0.25 * m2; |
| 78 | + value[0, 1] = 0.5 * m2 / s; |
| 79 | + value[0, 2] = 0.5 * m2 / s2; |
| 80 | + value[1, 0] = 0.5 * m2 / s; |
| 81 | + value[1, 2] = 1 * m2 / s3; |
| 82 | + value[2, 0] = 0.5 * m2 / s2; |
| 83 | + value[2, 1] = 1 * m2 / s4; |
| 84 | + value[3, 3] = 0.25 * m2; |
| 85 | + value[3, 4] = 0.5 * m2 / s; |
| 86 | + value[3, 5] = 0.5 * m2 / s2; |
| 87 | + value[4, 3] = 0.5 * m2 / s; |
| 88 | + value[4, 5] = 1 * m2 / s3; |
| 89 | + value[5, 3] = 0.5 * m2 / s2; |
| 90 | + value[5, 4] = 1 * m2 / s4; |
| 91 | + return 0.2 * 0.2 * value; |
| 92 | + }()}, |
| 93 | + output_uncertainty{{9. * m2, 0. * m2}, {0. * m2, 9. * m2}}, |
| 94 | + output_model{[]() { |
| 95 | + output_model::type value{zero<output_model::type>}; |
| 96 | + value[0, 0] = 1. * m2; |
| 97 | + value[1, 3] = 1. * m2; |
| 98 | + return value; |
| 99 | + }()}, |
| 100 | + state_transition{[]() { |
| 101 | + state_transition::type value{identity<state_transition::type>}; |
| 102 | + value[0, 1] = 1. * m2 / s; |
| 103 | + value[0, 2] = 0.5 * m2 / s2; |
| 104 | + value[1, 2] = 1. * m2 / s3; |
| 105 | + value[3, 4] = 1. * m2 / s; |
| 106 | + value[3, 5] = 0.5 * m2 / s2; |
| 107 | + value[4, 5] = 1. * m2 / s3; |
| 108 | + return value; |
| 109 | + }()}}; |
| 110 | + |
| 111 | + filter.predict(); |
| 112 | + filter.update(-393.66 * m, 300.4 * m); |
| 113 | + filter.predict(); |
| 114 | + filter.update(-375.93 * m, 301.78 * m); |
| 115 | + filter.predict(); |
| 116 | + |
| 117 | + // Verify the example estimated state at 0.1% accuracy. |
| 118 | + assert(abs(1 - filter.x<0>() / (-277.8 * m)) < 0.001 && |
| 119 | + abs(1 - filter.x<1>() / (148.3 * m / s)) < 0.001 && |
| 120 | + abs(1 - filter.x<2>() / (94.5 * m / s2)) < 0.001 && |
| 121 | + abs(1 - filter.x<3>() / (249.8 * m)) < 0.001 && |
| 122 | + abs(1 - filter.x<4>() / (-85.9 * m / s)) < 0.001 && |
| 123 | + abs(1 - filter.x<5>() / (-63.62 * m / s2)) < 0.001 && |
| 124 | + "The state estimates expected at 0.1% accuracy."); |
| 125 | + |
| 126 | + return 0; |
| 127 | +}()}; |
| 128 | +} // namespace |
| 129 | +} // namespace fcarouge::test |
0 commit comments