From 2872ee9546912d21d74356c3a9c0826331630c9f Mon Sep 17 00:00:00 2001 From: zhaoxi <535394140@qq.com> Date: Wed, 17 Apr 2024 21:42:04 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=89=A9=E5=B1=95=E5=8D=A1?= =?UTF-8?q?=E5=B0=94=E6=9B=BC=E6=BB=A4=E6=B3=A2=E6=A8=A1=E5=9D=97=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E6=B7=BB=E5=8A=A0=E7=9B=B8=E5=85=B3=E6=96=87=E6=A1=A3?= =?UTF-8?q?=E5=92=8C=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 6 +- cmake/RMVLUtils.cmake | 29 +- doc/header.html | 2 +- doc/tutorials/modules/core/ekf.md | 183 +++++++++++ doc/tutorials/modules/core/kalman.md | 34 +- doc/tutorials/modules/core/least_square.md | 6 +- doc/tutorials/modules/tutorial_modules.md | 2 +- extra/compensator/test/test_gravity.cpp | 4 +- modules/core/CMakeLists.txt | 8 + modules/core/include/rmvl/core/kalman.hpp | 350 ++++++++++++++------- modules/core/perf/perf_kalman.cpp | 82 +++++ modules/core/test/test_kalman.cpp | 150 +++++++++ modules/core/test/test_numcal.cpp | 37 +-- modules/imgproc/perf/perf_pretreat.cpp | 10 +- modules/light/CMakeLists.txt | 11 +- 15 files changed, 723 insertions(+), 191 deletions(-) create mode 100644 doc/tutorials/modules/core/ekf.md create mode 100644 modules/core/perf/perf_kalman.cpp create mode 100644 modules/core/test/test_kalman.cpp diff --git a/README.md b/README.md index 51e71ce1..d174c6ea 100755 --- a/README.md +++ b/README.md @@ -2,9 +2,9 @@ **机器人控制与视觉库** -| 构建配置 | 编译器/环境 | Github Actions 工作流状态 | -| :------: | :---------: | :----------------------------------------------------------: | -| CMake | GCC 12.3.0 | [![1.x in Linux](https://github.com/cv-rmvl/rmvl/actions/workflows/linux-1.x.yml/badge.svg)](https://github.com/cv-rmvl/rmvl/actions/workflows/linux-1.x.yml) | +| 构建配置 | 编译器/环境 | Github Actions 工作流状态 | +| :------: | :-------------------------------: | :--------------------------------------: | +| CMake | GCC 7.5.0 x86_64-linux-gnu
GCC 9.4.0 x86_64-linux-gnu
GCC 12.3.0 x86_64-linux-gnu | [![1.x in Linux](https://github.com/cv-rmvl/rmvl/actions/workflows/linux-1.x.yml/badge.svg)](https://github.com/cv-rmvl/rmvl/actions/workflows/linux-1.x.yml) | RMVL 最初是面向 RoboMaster 赛事的视觉库，因此称为 RoboMaster Vision Library，现计划并逐步完善与机器人相关的视觉、控制、通信的功能，旨在打造适用范围广、使用简洁、架构统一、功能强大的视觉控制一体库。 diff --git a/cmake/RMVLUtils.cmake b/cmake/RMVLUtils.cmake index 640dedb3..a82d42b0 100644 --- a/cmake/RMVLUtils.cmake +++ b/cmake/RMVLUtils.cmake @@ -183,7 +183,7 @@ function(status text) endfunction() # ---------------------------------------------------------------------------- -# 下载并参与 RMVL 的构建 +# 下载并参与 RMVL 的构建，支持 URL 和 GIT # 用法: # rmvl_download( # @@ -191,30 +191,19 @@ endfunction() # ) # 示例 1: # rmvl_download( -# googletest URL -# xxx +# googletest URL "xxx" # ) # 示例 2: # rmvl_download( -# benchmark GIT -# xxx : master +# benchmark GIT "xxx : master" # ) # ---------------------------------------------------------------------------- -function(rmvl_download dl_name dl_kind) - if(NOT "${dl_kind}" MATCHES "^(GIT|URL)$") - message(FATAL_ERROR "Unknown download kind : ${dl_kind}") - endif() +function(rmvl_download dl_name dl_kind dl_info) include(FetchContent) string(TOLOWER "${dl_kind}" dl_kind_lower) - # get url-string - list(LENGTH ARGN argn_length) - if(NOT argn_length EQUAL 1) - message(FATAL_ERROR "Argument \"\$\{ARGN\}\" count must be 1 in the function \"rmvl_download\"") - endif() - list(GET ARGN 0 web_url) # use git if("${dl_kind_lower}" STREQUAL "git") - string(REGEX MATCH "(.*) : (.*)" RESULT "${web_url}") + string(REGEX MATCH "(.*) : (.*)" RESULT "${dl_info}") set(git_url "${CMAKE_MATCH_1}") set(git_tag "${CMAKE_MATCH_2}") message(STATUS "Download ${dl_name} from \"${git_url}\" (tag: ${git_tag})") @@ -224,12 +213,14 @@ function(rmvl_download dl_name dl_kind) GIT_TAG ${git_tag} ) # use url - else() - message(STATUS "Download ${dl_name} from \"${web_url}\"") + elseif("${dl_kind_lower}" STREQUAL "url") + message(STATUS "Download ${dl_name} from \"${dl_info}\"") FetchContent_Declare( ${dl_name} - URL ${web_url} + URL ${dl_info} ) + else() + message(FATAL_ERROR "Unknown download kind : ${dl_kind}") endif() # make available FetchContent_MakeAvailable(${dl_name}) diff --git a/doc/header.html b/doc/header.html index 130b1c83..06166eab 100644 --- a/doc/header.html +++ b/doc/header.html @@ -24,7 +24,7 @@
- + diff --git a/doc/tutorials/modules/core/ekf.md b/doc/tutorials/modules/core/ekf.md new file mode 100644 index 00000000..6a7691c9 --- /dev/null +++ b/doc/tutorials/modules/core/ekf.md @@ -0,0 +1,183 @@ +扩展卡尔曼滤波 {#tutorial_modules_ekf} +============ + +@author 赵曦 +@date 2024/04/19 +@version 1.0 +@brief 扩展卡尔曼滤波 + +@prev_tutorial{tutorial_modules_kalman} + +@next_tutorial{tutorial_modules_union_find} + +@tableofcontents + +------ + +相关类 rm::ExtendedKalmanFilter + +\f[ +\def\red#1{\color{red}{#1}} +\def\teal#1{\color{teal}{#1}} +\def\green#1{\color{green}{#1}} +\def\transparent#1{\color{transparent}{#1}} +\def\orange#1{\color{orange}{#1}} +\def\Var{\mathrm{Var}} +\def\Cov{\mathrm{Cov}} +\def\tr{\mathrm{tr}} +\def\fml#1{\text{(#1)}} +\def\ptl#1#2{\frac{\partial#1}{\partial#2}} +\f] + +在阅读本教程前，请确保已经熟悉标准的 @ref tutorial_modules_kalman ，因为核心公式不变，只是在原来的基础上增加了非线性函数线性化的部分。 + +### 1. 非线性函数的线性化 + +对于一个线性系统，可以用状态空间方程描述其运动过程 + +\f[\begin{align}\dot{\pmb x}&=A\pmb x+B\pmb u\\\pmb y&=C\pmb x\end{align}\tag{1-1}\f] + +离散化，并考虑噪声后可以写为 + +\f[\begin{align}\dot{\pmb x}_k&=A\pmb x_{k-1}+B\pmb u_{k-1}+\pmb w_{k-1}&&\pmb w_{k-1}\sim N(0,Q)\tag{1-2a}\\ +\pmb z_k&=H\pmb x_{k-1}+\pmb v_k&&\pmb v_k\sim N(0,R)\tag{1-2b}\end{align}\f] + +但对于一个非线性系统，我们就无法使用矩阵来表示了，我们需要写为 + +\f[\left\{\begin{align}\dot{\pmb x}_k&=\pmb f_A(\pmb x_{k-1},\pmb u_{k-1},\pmb w_{k-1})\\ +\pmb z_k&=\pmb f_H(\pmb x_{k-1},\pmb v_{k-1})\end{align}\right.\tag{1-3}\f] + +其中，\f$\pmb f_A\f$ 和 \f$\pmb f_H\f$ 都为非线性函数。我们在非线性函数中同样考虑了噪声，但是对于状态量以及观测量本身的噪声而言，正态分布的随机变量通过非线性系统后就不再服从正态分布了。因此我们可以利用 **泰勒展开** ，将非线性系统线性化，即 + +\f[f(x)\approx f(x_0)+\frac{\mathrm df}{\mathrm dx}(x-x_0)\tag{1-4}\f] + +对于多元函数而言，泰勒展开可以写为 + +\f[f(x,y,z)\approx f(x_0,y_0,z_0)+\begin{bmatrix}f'_x(x_0,y_0,z_0)&f'_y(x_0,y_0,z_0)&f'_z(x_0,y_0,z_0)\end{bmatrix}\begin{bmatrix}x-x_0\\y-y_0\\z-z_0\end{bmatrix}\tag{1-5a}\f] + +即 + +\f[f(\pmb x)\approx f(\pmb x_0)+\ptl fx(\pmb x-\pmb x_0)=f(\pmb x_0)+\nabla f(\pmb x_0)(\pmb x-\pmb x_0)\tag{1-5b}\f] + +#### 1.1 状态方程线性化 {#ekf_state_function_linearization} + +对公式 \f$\fml{1-2a}\f$ 在 \f$\hat{\pmb x}_{k-1}\f$ 处进行线性化，即选取 \f$\text{k-1}\f$ 时刻的后验状态估计作为展开点，有 + +\f[\pmb x_k=\pmb f_A(\hat{\pmb x}_{k-1},\pmb u_{k-1},\pmb w_{k-1})+J_A(\pmb x_{k-1}-\hat x_{k-1})+W\pmb w_{k-1}\tag{1-6}\f] + +令 \f$\pmb w_{k-1}=\pmb 0\f$，则 \f$f_A(\hat{\pmb x}_{k-1},\pmb u_{k-1},\pmb w_{k-1})=f_A(\hat{\pmb x}_{k-1},\pmb u_{k-1},\pmb 0)\stackrel{\triangle}=\tilde{\pmb x}_{k-1}\f$，有 + +\f[\red{\pmb x_k=\tilde{\pmb x}_{k-1}+J_A(\pmb x_{k-1}-\hat x_{k-1})+W\pmb w_{k-1}\qquad W\pmb w_{k-1}\sim N(0,WQW^T)\tag{1-7}}\f] + +其中 + +\f[\begin{align}J_A&=\left.\ptl{f_A}{\pmb x}\right|_{(\hat{\pmb x}_{k-1},\pmb u_{k-1})}=\begin{bmatrix}\ptl{{f_A}_1}{x_1}&\ptl{{f_A}_1}{x_2}&\cdots&\ptl{{f_A}_1}{x_n}\\\ptl{{f_A}_2}{x_1}&\ptl{{f_A}_2}{x_2}&\cdots&\ptl{{f_A}_2}{x_n}\\\vdots&\vdots&\ddots&\vdots\\\ptl{{f_A}_n}{x_1}&\ptl{{f_A}_n}{x_2}&\cdots&\ptl{{f_A}_n}{x_n}\end{bmatrix}\\ +W&=\left.\ptl{f_A}{\pmb w}\right|_{(\hat{\pmb w}_{k-1},\pmb u_{k-1})}\end{align}\f] + +#### 1.2 观测方程线性化 {#ekf_observation_function_linearization} + +对公式 \f$\fml{1-2b}\f$ 在 \f$\hat{\pmb x}_k\f$ 处进行线性化，有 + +\f[\pmb z_k=\pmb f_H(\tilde{\pmb x}_k,\pmb v_k)+J_H(\pmb x_k-\tilde x_k)+V\pmb v_k\tag{1-8}\f] + +令 \f$\pmb v_k=\pmb 0\f$，则 \f$f_H(\tilde{\pmb x}_k,\pmb v_k)=f_H(\tilde{\pmb x}_k,\pmb 0)\stackrel{\triangle}=\tilde{\pmb z}_k\f$，有 + +\f[\red{\pmb z_k=\tilde{\pmb z}_k+J_H(\pmb x_k-\tilde x_k)+V\pmb v_k\qquad V\pmb v_k\sim N(0,VRV^T)\tag{1-9}}\f] + +其中 + +\f[J_H=\left.\ptl{f_H}{\pmb x}\right|_{\tilde{\pmb x}_k},\qquad V=\left.\ptl{f_H}{\pmb v}\right|_{\tilde{\pmb x}_k}\f] + +### 2. 扩展卡尔曼滤波 + +#### 2.1 公式汇总 + +根据卡尔曼滤波的 @ref kalman_filter_formulas 可以相应的改写非线性系统下的卡尔曼滤波公式，从而得到如下的扩展卡尔曼滤波公式。 + +**① 预测** + +1. 先验状态估计 + \f[\hat{\pmb x}_k^-=\pmb f_A(\pmb x_{k-1},\pmb u_{k-1},\pmb 0)\f] + +2. 计算先验误差协方差 + \f[P_k^-=J_AP_{k-1}J_A^T+WQW^T\f] + +**② 校正（更新）** + +1. 计算卡尔曼增益 + \f[K_k=P_k^-J_H^T\left(J_HP_k^-J_H^T+VRV^T\right)^{-1}\f] + +2. 后验状态估计 + \f[\hat{\pmb x}_k=\hat{\pmb x}_k^-+K_k\left[\pmb z_k-\pmb f_H(\hat{\pmb x}_k^-,\pmb 0)\right]\f] + +3. 更新后验误差协方差 + \f[P_k=(I-K_kJ_H)P_k^-\f] + +#### 2.2 EKF 模块的使用 + +下面拿扩展卡尔曼模块单元测试的内容举例子 + +```cpp +#include +#include +#include + +int main() +{ + // 状态量 x = [ cx, cy, θ, ω, r ]ᵀ + // ┌ cx ┌ 1 0 0 0 0 ┐ + // │ cy │ 0 1 0 0 0 │ + // 状态方程 F = │ θ + ωT Ja = │ 0 0 1 T 0 │ = A + // │ ω │ 0 0 0 1 0 │ + // └ r └ 0 0 0 0 1 ┘ + // 观测量 z = [ px, py, θ ]ᵀ + // ┌ cx + rcosθ ┌ 1 0 -rsinθ 0 cosθ ┐ + // 观测方程 H = │ cy + rsinθ Jh = │ 0 1 rcosθ 0 sinθ │ + // └ θ └ 0 0 1 0 0 ┘ + + // 正态分布噪声 + std::default_random_engine ng; + std::normal_distribution err{0, 1}; + + // 创建扩展卡尔曼滤波 + rm::EKF53d ekf; + ekf.init({0, 0, 0, 0, 150}, 1e5); + ekf.setQ(1e-1 * cv::Matx::eye()); + ekf.setR(cv::Matx33d::diag({1e-3, 1e-3, 1e-3})); + double t{0.01}; + // 设置状态方程（这里的例子是线性的，但一般都是非线性的） + ekf.setFa([=](const cv::Matx &x) -> cv::Matx { + return {x(0), + x(1), + x(2) + x(3) * t, + x(3), + x(4)}; + }); + // 设置观测方程 + ekf.setFh([=](const cv::Matx &x) -> cv::Matx { + return {x(0) + x(4) * std::cos(x(2)), + x(1) + x(4) * std::sin(x(2)), + x(2)}; + }); + + while (true) + { + // 预测部分，设置状态方程 Jacobi 矩阵，获取先验状态估计 + ekf.setJa({1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, t, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1}); + auto x_ = ekf.predict(); + // 更新部分，设置观测方程 Jacobi 矩阵，获取后验状态估计 + ekf.setJh({1, 0, -x_(4) * std::sin(x_(2)), 0, std::cos(x_(2)), + 0, 1, x_(4) * std::cos(x_(2)), 0, std::sin(x_(2)), + 0, 0, 1, 0, 0}); + // 以 20 为半径，0.02/T 为角速度的圆周运动（图像上是顺时针），并人为加上观测噪声 + auto x = ekf.correct({500 + 200 * std::cos(0.02 * i) + err(ng), + 500 + 200 * std::sin(0.02 * i) + err(ng), + 0.02 * i + 0.01 * err(ng)}); + printf("x = [%.3f, %.3f, %.3f, %.3f, %.3f]\n", x(0), x(1), x(2), x(3), x(4)); + } +} +``` diff --git a/doc/tutorials/modules/core/kalman.md b/doc/tutorials/modules/core/kalman.md index b814d086..90649055 100644 --- a/doc/tutorials/modules/core/kalman.md +++ b/doc/tutorials/modules/core/kalman.md @@ -9,7 +9,7 @@ @prev_tutorial{tutorial_modules_runge_kutta} -@next_tutorial{tutorial_modules_union_find} +@next_tutorial{tutorial_modules_ekf} @tableofcontents @@ -26,7 +26,7 @@ \def\Var{\mathrm{Var}} \def\Cov{\mathrm{Cov}} \def\tr{\mathrm{tr}} -\def\formular#1{\text{(#1)}} +\def\fml#1{\text{(#1)}} \f] ### 1. 卡尔曼滤波 @@ -47,9 +47,9 @@ \f[\hat x_k=\frac{z_1+z_2+\cdots+z_k}k\tag{1-1}\f] -这是一条非常简单的取算数平均的公式，但是我们为了估计硬币的长度，需要用到所有的观测值，例如我们已经测了 5 次硬币的长度，并且使用公式 \f$\formular{1-1}\f$ 得到了第 5 次的平均值（长度的估计值），在测完第 6 次长度准备计算第 6 次估计值的时候，使用公式 \f$\formular{1-1}\f$ 还需要重新使用前 5 次的观测值。当观测次数非常高的时候，计算的压力就逐渐高起来了。 +这是一条非常简单的取算数平均的公式，但是我们为了估计硬币的长度，需要用到所有的观测值，例如我们已经测了 5 次硬币的长度，并且使用公式 \f$\fml{1-1}\f$ 得到了第 5 次的平均值（长度的估计值），在测完第 6 次长度准备计算第 6 次估计值的时候，使用公式 \f$\fml{1-1}\f$ 还需要重新使用前 5 次的观测值。当观测次数非常高的时候，计算的压力就逐渐高起来了。 -针对这一问题，我们可以改写公式 \f$\formular{1-1}\f$ +针对这一问题，我们可以改写公式 \f$\fml{1-1}\f$ \f[\begin{align}\hat x_k&=\frac1k(z_1+z_2+\cdots+z_k)\\ &=\frac1k(z_1+z_2+\cdots+z_{k-1})+\frac1kz_k\\ @@ -59,7 +59,7 @@ 这样我们就把硬币长度的估计值，改写成由上一次估计值和当前观测值共同作用的形式。并且我们发现，随着测量次数 \f$k\f$ 增大，\f$\frac1k\f$ 趋向于 \f$0\f$，\f$\hat x_k\f$ 趋向于 \f$\hat x_{k-1}\f$，这也就是说，随着 \f$k\f$ 增长，测量结果将不再重要。 -为了不失一般性，我们把公式 \f$\formular{1-2a}\f$ 的结果改写成以下形式。 +为了不失一般性，我们把公式 \f$\fml{1-2a}\f$ 的结果改写成以下形式。 \f[\boxed{\hat x_k=\hat x_{k-1}+\red{K_k}(z_k-\hat x_{k-1})}\tag{1-2b}\f] @@ -74,7 +74,7 @@ #### 1.3 数据融合 {#kalman_data_fusion} -现在我们可以使用公式 \f$\formular{1-2b}\f$ 的思想来研究数据融合。如果有一辆车以恒定速度行驶，现在得到了两个数据： +现在我们可以使用公式 \f$\fml{1-2b}\f$ 的思想来研究数据融合。如果有一辆车以恒定速度行驶，现在得到了两个数据： - 根据匀速公式计算得到的汽车当前位置（算出来的，记作 \f$\teal{x_1}\f$） - 汽车的当前距离传感器数据（测出来的，记作 \f$\red{x_2}\f$） @@ -83,7 +83,7 @@ 由于自身的原因，它的位置并不是由匀速运动公式得到的精确位置，它的距离传感器数据也不完全准确。两者都有一定的误差。那么我们现在如何估计汽车的实际位置呢？ -我们使用公式 \f$\formular{1-2b}\f$ 的思想，得到估计值 +我们使用公式 \f$\fml{1-2b}\f$ 的思想，得到估计值 \f[\hat x=\teal{x_1}+\green{K_k}(\red{x_2}-\teal{x_1})\tag{1-4}\f] @@ -219,7 +219,7 @@ e_ne_1&e_ne_2&\cdots&e_n^2\end{bmatrix}=\green{E\left(\pmb e\pmb e^T\right)}\tag \pmb z_k&=H\pmb x_k \end{align}\right.\tag{1-13b}\f] -这其实是个不准确的结果，因为如果我们考虑上噪声，公式 \f$\formular{1-13b}\f$ 应该改写为 +这其实是个不准确的结果，因为如果我们考虑上噪声，公式 \f$\fml{1-13b}\f$ 应该改写为 \f[\left\{\begin{align} \pmb x_k&=A\pmb x_{k-1}+B\pmb u_{k-1}\red{+\pmb w_{k-1}}&p(\pmb w)\sim N(0,Q)\\ @@ -237,7 +237,7 @@ e_ne_1&e_ne_2&\cdots&e_n^2\end{bmatrix}=\green{E\left(\pmb e\pmb e^T\right)}\tag \vdots&\vdots&\ddots&\vdots\\\sigma_{v_n}\sigma_{v_1}&\sigma_{v_n}\sigma_{v_2}&\cdots&\sigma_{v_n}^2\end{bmatrix}\f] 称为测量噪声协方差矩阵 -但是，这两个误差我们无从得知，我们只能使用公式 \f$\formular{1-13b}\f$ 的形式进行近似估计，通过 \f$\pmb x_k=A\pmb x_{k-1}+B\pmb u_{k-1}\f$ 算出来的 \f$\pmb x_k\f$ 称为先验状态估计，一般写为 +但是，这两个误差我们无从得知，我们只能使用公式 \f$\fml{1-13b}\f$ 的形式进行近似估计，通过 \f$\pmb x_k=A\pmb x_{k-1}+B\pmb u_{k-1}\f$ 算出来的 \f$\pmb x_k\f$ 称为先验状态估计，一般写为 \f[\red{\hat{\pmb x}_k^-=A\pmb x_{k-1}+B\pmb u_{k-1}\tag{1-14}}\f] @@ -249,7 +249,7 @@ e_ne_1&e_ne_2&\cdots&e_n^2\end{bmatrix}=\green{E\left(\pmb e\pmb e^T\right)}\tag @note 对于一个线性方程组 \f[A\pmb x=\pmb b\f]必定存在最小二乘解 \f[\pmb x=(A^TA)^{-1}A^T\pmb b\f]可以令 \f$A^+=(A^TA)^{-1}A^T\f$ 来表示 Moore-Penrose 广义逆，即\f[\pmb x=A^+\pmb b\f]当 \f$A\f$ 可逆时，\f$A^+=A^{-1}\f$. -目前的两个结果 \f$\hat{\pmb x}_k^-\f$ 和 \f$\hat{\pmb x}_{k_{MEA}}\f$ 都不准确，因此可以回顾 @ref kalman_data_fusion 的部分，在公式 \f$\formular{1-4}\f$ 中使用了算出来的 \f$\teal{x_1}\f$ 和测出来的 \f$\red{x_2}\f$ 得到了最优估计值 \f$\hat x\f$，为此我们可以仿照这一步骤来求出离散系统状态的最优估计值 \f$\hat{\pmb x}_k\f$，称为后验状态估计。 +目前的两个结果 \f$\hat{\pmb x}_k^-\f$ 和 \f$\hat{\pmb x}_{k_{MEA}}\f$ 都不准确，因此可以回顾 @ref kalman_data_fusion 的部分，在公式 \f$\fml{1-4}\f$ 中使用了算出来的 \f$\teal{x_1}\f$ 和测出来的 \f$\red{x_2}\f$ 得到了最优估计值 \f$\hat x\f$，为此我们可以仿照这一步骤来求出离散系统状态的最优估计值 \f$\hat{\pmb x}_k\f$，称为后验状态估计。 \f[\begin{align}\hat{\pmb x}_k&=\hat{\pmb x}_k^-+\green{G_k}(\hat{\pmb x}_{k_{MEA}}-\hat{\pmb x}_k^-)\\ &=\hat{\pmb x}_k^-+\green{G_k}(H^+\pmb z_k-\hat{\pmb x}_k^-)\tag{1-16}\end{align}\f] @@ -324,7 +324,7 @@ P_k^-对称\quad&=P_k^--K_kHP_k^--\left(K_kHP_k^-\right)^T+K_kHP_k^-H^TK_k^T+K_k \f[\begin{align}\frac{\mathrm d\tr(ABA^T)}{A}&=AB+AB^T\\ 当B对称时\quad&=2AB\end{align}\tag{1-23b}\f] -那么，公式\f$\formular{1-22}\f$可以写为 +那么，公式\f$\fml{1-22}\f$可以写为 \f[\begin{align}2\frac{\mathrm d\tr(K_kHP_k^-)}{\mathrm dK_k} &=\frac{\mathrm d\tr(K_kHP_k^-H^TK_k^T)}{\mathrm dK_k}+\frac{\mathrm d\tr(K_kRK_k^T)}{\mathrm dK_k}\\ @@ -368,21 +368,21 @@ P_k^-H^T&=K_k\left(HP_k^-H^T+R\right) 在求解 \f$P_k^-\f$ 的时候用到了 \f$P_{k-1}\f$，因此需要进一步求解 \f$P_k\f$，从而为下一次 \f$P_{k+1}^-\f$ 所使用。 -由 @ref kalman_gain_derivate 的公式 \f$\formular{1-20}\f$ 可以得到 +由 @ref kalman_gain_derivate 的公式 \f$\fml{1-20}\f$ 可以得到 \f[\begin{align}P_k &=\green{P_k^-}-\red{K_kHP_k^-}-\green{P_k^-}\orange{H^TK_k^T}+\red{K_kHP_k^-}\orange{H^TK_k^T}+K_kRK_k^T\\ &=P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_k(HP_k^-H^T+R)K_k^T\\ -代入\formular{1-25}\quad&=P_k^--K_kHP_k^--P_k^-H^TK_k^T+P_k^-H^TK_k^T\\ +代入\fml{1-25}\quad&=P_k^--K_kHP_k^--P_k^-H^TK_k^T+P_k^-H^TK_k^T\\ &=P_k^--K_kHP_k^-\tag{1-29}\end{align}\f] 即所谓后验误差协方差矩阵 \f$P_k\f$ \f[\red{P_k=(I-K_kH)P_k^-\tag{1-30}}\f] -#### 1.7 汇总 {#kalman_filter_fomulars} +#### 1.7 汇总 {#kalman_filter_formulas} -至此，Kalman Filter 的 5 大公式已经全部求出，分别是公式 \f$\formular{1-14}\f$、公式 \f$\formular{1-17}\f$、公式 \f$\formular{1-25}\f$、公式 \f$\formular{1-28}\f$ 和公式 \f$\formular{1-30}\f$ +至此，Kalman Filter 的 5 大公式已经全部求出，分别是公式 \f$\fml{1-14}\f$、公式 \f$\fml{1-17}\f$、公式 \f$\fml{1-25}\f$、公式 \f$\fml{1-28}\f$ 和公式 \f$\fml{1-30}\f$ 按照处理顺序，卡尔曼滤波器划分为两个部分 @@ -411,7 +411,7 @@ P_k^-H^T&=K_k\left(HP_k^-H^T+R\right) #### 2.1 如何配置 -首先必须要寻找 RMVL 包，即 `find_package(RMVL [OPTIONS])`，之后可直接在中使用在 CMakeLists.txt 中链接库 +首先必须要寻找 RMVL 包，即 `find_package(RMVL REQUIRED)`，之后可直接在 CMakeLists.txt 中链接库 ```cmake target_link_libraries( @@ -420,6 +420,8 @@ target_link_libraries( ) ``` +这里的 `xxx` 为需要链接到 core 模块的目标 + #### 2.2 如何使用 ##### 2.2.1 包含头文件 diff --git a/doc/tutorials/modules/core/least_square.md b/doc/tutorials/modules/core/least_square.md index a809962b..ab5c7f12 100644 --- a/doc/tutorials/modules/core/least_square.md +++ b/doc/tutorials/modules/core/least_square.md @@ -147,7 +147,7 @@ A^TA\hat{\pmb x}&=A^T\pmb b 这就是最小二乘解所满足的代数方程。 -### 3. 示例 +#### 示例 下面给出 3 个示例，不直接使用公式 \f$\text{(2-7)}\f$，通过几何或者其他手段来表示最小二乘解。 @@ -293,7 +293,7 @@ f(t_0)\\f(t_1)\\\vdots\\f(t_{s-1})\end{bmatrix}\tag{3-11}\f] 后文将给出另外一种描述最小二乘法的做法，这种做法有别于上面构造向量与子空间垂直的方式，通过对误差平方和直接求其最小值来得到最小二乘解（两种方式结果均能推导出公式 \f$\text{(2-7)}\f$，但出发点不同）。此外，要介绍的这个解法也同样适用于整个线性空间（包括欧式空间、多项式空间等线性空间）。 -### 4. 法方程求解最小二乘法 +### 3. 法方程求解最小二乘法 相关类 rm::CurveFitter @@ -351,7 +351,7 @@ f(t_0)\\f(t_1)\\\vdots\\f(t_{s-1})\end{bmatrix}\tag{3-11}\f] 对于 \f$(\phi_p,\phi_q)\f$，依照公式\f$\text{(4-4)}\f$，可以写成矩阵的表示方式，即 \f[\begin{align}(\phi_p,\phi_q)&=\sum_{i=0}^{s-1}\phi_p(t_i)\phi_q(t_i)\\&=[\phi_p(t_0),\phi_p(t_1),\cdots,\phi_p(t_{s-1})] -\begin{bmatrix}\phi_i(t_0)\\\phi_i(t_1)\\\vdots\\\phi_q(t_{s-1})\end{bmatrix}\end{align}\tag{4-7}\f] +\begin{bmatrix}\phi_q(t_0)\\\phi_q(t_1)\\\vdots\\\phi_q(t_{s-1})\end{bmatrix}\end{align}\tag{4-7}\f] 因此对法方程系数矩阵 \f$G\f$ 的第 \f$k\ (k=0,1,\cdots,n-1)\f$ 行，有 diff --git a/doc/tutorials/modules/tutorial_modules.md b/doc/tutorials/modules/tutorial_modules.md index 42d998b8..72363f63 100644 --- a/doc/tutorials/modules/tutorial_modules.md +++ b/doc/tutorials/modules/tutorial_modules.md @@ -28,7 +28,7 @@ ##### 数据处理 -- @subpage tutorial_modules_kalman +- @subpage tutorial_modules_kalman 和 @subpage tutorial_modules_ekf ### 2. 数据结构与算法 diff --git a/extra/compensator/test/test_gravity.cpp b/extra/compensator/test/test_gravity.cpp index 58b4a20c..143e98f7 100644 --- a/extra/compensator/test/test_gravity.cpp +++ b/extra/compensator/test/test_gravity.cpp @@ -34,8 +34,8 @@ TEST(GravityCompensator, bulletModel) // 无阻力情况下 double t = 10 / (16 * cos(rm::PI_4)); double y = 16 * sin(rm::PI_4) * t - 0.5 * rm::para::gravity_compensator_param.g * t * t; - EXPECT_LE(std::abs(y - y_fric), 5e-2); - EXPECT_LE(std::abs(t - t_fric), 5e-2); + EXPECT_NEAR(y, y_fric, 5e-2); + EXPECT_NEAR(t, t_fric, 5e-2); } } // namespace rm_test diff --git a/modules/core/CMakeLists.txt b/modules/core/CMakeLists.txt index ec19f1f8..8c9a75b6 100644 --- a/modules/core/CMakeLists.txt +++ b/modules/core/CMakeLists.txt @@ -30,3 +30,11 @@ if(BUILD_TESTS) DEPEND_TESTS GTest::gtest_main ) endif(BUILD_TESTS) + +if(BUILD_PERF_TESTS) + rmvl_add_test( + core Performance + DEPENDS core + DEPEND_TESTS benchmark::benchmark_main + ) +endif(BUILD_PERF_TESTS) diff --git a/modules/core/include/rmvl/core/kalman.hpp b/modules/core/include/rmvl/core/kalman.hpp index 5ac916b5..16a37db2 100755 --- a/modules/core/include/rmvl/core/kalman.hpp +++ b/modules/core/include/rmvl/core/kalman.hpp @@ -11,65 +11,54 @@ #pragma once +#include + #include //! @addtogroup core //! @{ -//! @defgroup core_kalman 卡尔曼滤波器库 +//! @defgroup core_kalman 卡尔曼滤波模块 //! @{ -//! @brief 使用 `cv::Matx` 改写的轻量级卡尔曼滤波模块 +//! @brief 使用 `cv::Matx` 改写的轻量级卡尔曼滤波和扩展卡尔曼滤波模块 //! @brief -//! - 考虑到 OpenCV 中提供的 `cv::KalmanFilter` 是基于 `cv::Mat` 实现的，并且 `cv::Mat` -//! 的内存操作在运行时是在堆上打开的，因此会消耗大量的时间，所以现使用 `cv::Matx` -//! 来复现卡尔曼滤波的功能。并简化部分功能的实现，以达到轻量化的目的。 +//! - 考虑到 OpenCV 中提供的 `cv::KalmanFilter` 是基于 `cv::Mat` 实现的，`cv::Mat` +//! 的内存操作在运行时是在堆上打开的，因此会消耗大量的时间，本模块使用 `cv::Matx` +//! 来实现 KF 和 EKF 的功能，以达到轻量化的目的。 //! @brief -//! - 相关知识点可参考说明文档 @ref tutorial_modules_kalman +//! - 相关知识点可参考说明文档 @ref tutorial_modules_kalman 以及 @ref tutorial_modules_ekf //! @} core_kalman //! @} core namespace rm { -//! @addtogroup core_kalman 卡尔曼滤波器库 +//! @addtogroup core_kalman //! @{ /** - * @brief 轻量级 `cv::Matx` 的卡尔曼滤波模块 + * @brief 卡尔曼滤波静态数据 * * @tparam Tp 数据类型 - * @tparam StateDim 状态向量的维度，类型是 `uint16_t` - * @tparam MeasureDim 观测向量的维度，类型是 `uint16_t` - * @tparam ControlDim 控制向量的维度，类型是 `uint16_t`，默认为 `1` + * @tparam StateDim 状态量个数 + * @tparam MeasureDim 观测量个数 */ -template -class KalmanFilter +template +class KalmanFilterStaticDatas { -#if __cplusplus >= 201703L - static_assert(std::is_floating_point_v, "\"Tp\" must be floating point value."); - static_assert(ControlDim != 0, "ControlDim of \"rm::KalmanFilter\" must greater than 0."); -#endif // C++17 - - cv::Matx A; //!< 状态转移矩阵 \f$A\f$ - cv::Matx At; //!< 状态转移矩阵 `A` 的转置矩阵 \f$A^T\f$ - cv::Matx x; //!< 状态的后验估计 \f$\hat x\f$ - cv::Matx x_; //!< 状态的先验估计 \f$\hat x^-\f$ - cv::Matx B; //!< 控制矩阵 `B` \f$B\f$ - cv::Matx u; //!< 控制向量 `u` \f$\vec{u}\f$ - - cv::Matx H; //!< 观测矩阵 \f$H\f$ - cv::Matx Ht; //!< 观测矩阵 `H` 的转置矩阵 \f$H^T\f$ +protected: + cv::Matx x; //!< 状态的后验估计 \f$\hat{\pmb x}\f$ + cv::Matx x_; //!< 状态的先验估计 \f$\hat{\pmb x}^-\f$ + cv::Matx z; //!< 观测向量 \f$\pmb z\f$ cv::Matx Q; //!< 过程噪声协方差矩阵 \f$Q\f$ cv::Matx R; //!< 测量噪声协方差矩阵 \f$R\f$ cv::Matx P; //!< 后验误差协方差矩阵 \f$P\f$ cv::Matx P_; //!< 先验误差协方差矩阵 \f$P^-\f$ - cv::Matx I; //!< 单位矩阵 `I` \f$I\f$ - cv::Matx K; //!< 卡尔曼增益 `K` \f$K\f$ - cv::Matx z; //!< 观测向量 `z` \f$\vec{z}\f$ + cv::Matx I; //!< 单位矩阵 \f$I\f$ + cv::Matx K; //!< 卡尔曼增益 \f$K\f$ public: - //! 构造新的 KalmanFilter 对象 - KalmanFilter() : A(A.eye()), At(At.eye()), H(H.eye()), Ht(Ht.eye()), - Q(Q.eye()), R(R.eye()), P(P.eye()), I(I.eye()) {} + //! 构造新的卡尔曼滤波静态数据 + KalmanFilterStaticDatas() : Q(Q.eye()), R(R.eye()), P(P.eye()), I(I.eye()) {} /** * @brief 初始化状态以及对应的误差协方差矩阵（常数对角矩阵） @@ -79,8 +68,8 @@ class KalmanFilter */ void init(const cv::Matx &x0, Tp error) { - x_ = x = x0; - P_ = P = P.eye() * error; + this->x_ = this->x = x0; + this->P_ = this->P = this->P.eye() * error; } /** @@ -91,13 +80,60 @@ class KalmanFilter */ void init(const cv::Matx &x0, const cv::Matx &error) { - x_ = x = x0; - P_ = P = P.diag(error); + this->x_ = this->x = x0; + this->P_ = this->P = this->P.diag(error); } /** - * @brief 设置状态转移矩阵 `A` - * @details + * @brief 设置测量噪声协方差矩阵 \f$R\f$ + * + * @param[in] measure_err 测量噪声协方差矩阵 \f$R\f$ + */ + inline void setR(const cv::Matx &measure_err) { R = measure_err; } + + /** + * @brief 设置过程噪声协方差矩阵 \f$Q\f$ + * + * @param[in] process_err 过程噪声协方差矩阵 \f$Q\f$ + */ + inline void setQ(const cv::Matx &process_err) { Q = process_err; } + + /** + * @brief 设置误差协方差矩阵 \f$P\f$ + * + * @param[in] state_err 误差协方差矩阵 \f$P\f$ + */ + inline void setP(const cv::Matx &state_err) { P_ = P = state_err; } +}; + +/** + * @brief 卡尔曼滤波器 + * + * @tparam Tp 数据类型 + * @tparam StateDim 状态量个数 + * @tparam MeasureDim 观测量个数 + */ +template +class KalmanFilter : public KalmanFilterStaticDatas +{ + static_assert(std::is_floating_point_v, "\"Tp\" must be floating point value."); + static_assert(StateDim > 0, "StateDim of \"rm::KalmanFilter\" must greater than 0."); + static_assert(MeasureDim > 0, "MeasureDim of \"rm::KalmanFilter\" must greater than 0."); + +protected: + cv::Matx A; //!< 状态转移矩阵 \f$A\f$ + cv::Matx At; //!< 状态转移矩阵的转置 \f$A^T\f$ + cv::Matx H; //!< 观测矩阵 \f$H\f$ + cv::Matx Ht; //!< 观测矩阵的转置 \f$H^T\f$ + +public: + //! 构造新的 KalmanFilter 对象 + KalmanFilter() : KalmanFilterStaticDatas(), + A(A.eye()), At(At.eye()), H(H.eye()), Ht(Ht.eye()) {} + + /** + * @brief 设置状态转移矩阵 \f$A\f$ + * @brief * 包含 `x` 方向位置、`y` 方向位置、`x` 方向速度和 `y` 方向速度的运动过程一般可以描述为 * \f[\left\{\begin{align}x_{n+1}&=x_n+{v_x}_nt+\frac12{a_x}_nt^2\\y_{n+1}&=y_n+{v_y}_nt * +\frac12{a_y}_nt^2\\{v_x}_{n+1}&={v_x}_n+{a_x}_nt\\{v_y}_{n+1}&={v_y}_n+{a_y}_nt @@ -115,127 +151,207 @@ class KalmanFilter * * @param[in] state_tf 状态转移矩阵 */ - inline void setA(const cv::Matx &state_tf) { A = state_tf, At = state_tf.t(); } + inline void setA(const cv::Matx &state_tf) { this->A = state_tf, this->At = state_tf.t(); } /** - * @brief 设置控制矩阵 `B` - * @details 输入直接对状态向量的作用，由控制矩阵 `B` 定义 - * @see setA + * @brief 设置观测矩阵 \f$H\f$ + * @brief + * 若状态向量包含以下内容：\f$[p, v, a]^T\f$ ，然而观测向量仅包含 \f$[p, v]^T\f$ ， + * 在这种情况下，需要使用一个观测矩阵 \f$H_{2\times3}\f$。在上述例子中可表示为 + * \f[\begin{bmatrix}p\\v\end{bmatrix}=\begin{bmatrix}1&0&0\\0&1&0\end{bmatrix} + * \begin{bmatrix}p\\v\\a\end{bmatrix}\f] + * @param[in] observe_tf 观测矩阵 + */ + inline void setH(const cv::Matx &observe_tf) { this->H = observe_tf, this->Ht = observe_tf.t(); } + + /** + * @brief 卡尔曼滤波的预测部分，包括状态量的先验估计和误差协方差的先验估计 + * @brief 公式如下 \f[\begin{align}\hat{\pmb x}_k^-&=A\hat{\pmb x}_{k-1}\\P_k^-&=AP_{k-1}A^T+Q\end{align}\f] * - * @param[in] control_matrix 控制矩阵 + * @return 先验状态估计 */ - inline void setB(const cv::Matx &control_matrix) { B = control_matrix; } + inline auto predict() + { + // 先验状态估计 + this->x_ = A * this->x; + // 先验误差协方差 + this->P_ = A * this->P * At + this->Q; + return this->x_; + } /** - * @brief 设置观测转换矩阵 `H` - * @details - * 若状态向量包含以下内容：\f$[p, v, a]^T\f$ ，然而观测向量仅包含 \f$[p, v]^T\f$， - * 在这种情况下，需要使用一个观测转换矩阵 \f$H_{2\times3}\f$。在上述例子中可表示为 - * \f[\begin{bmatrix}p\\v\end{bmatrix}=\begin{bmatrix}1&0&0\\0&1&0\end{bmatrix} - * \begin{bmatrix}p\\v\\a\end{bmatrix}\f] - * @note `H` 若不加以设置，则默认是单位矩阵 \f$I\f$ - * @param[in] observe_tf 观测转换矩阵 + * @brief 卡尔曼滤波器校正部分，包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计 + * @brief 公式如下 \f[\begin{align}K_k&=P_k^-H^T\left(HP_k^-H^T+R\right)^{-1}\\\hat{\pmb x}_k&=\hat{\pmb + * x}_k^-+K\left(\pmb z_k-H\hat{\pmb x}_k^-\right)\\P_k&=\left(I-K_kH\right)P_k^-\end{align}\f] + * + * @param[in] zk 观测量 + * @return 后验状态估计 + */ + inline auto correct(const cv::Matx &zk) + { + this->z = zk; + // 计算卡尔曼增益 + this->K = this->P_ * Ht * (H * this->P_ * Ht + this->R).inv(); + // 后验状态估计 + this->x = this->x_ + this->K * (this->z - this->H * this->x_); + // 后验误差协方差 + this->P = (this->I - this->K * H) * this->P_; + return this->x; + } +}; + +using KF21f = KalmanFilter; //!< 2 × 1 卡尔曼滤波器 +using KF21d = KalmanFilter; //!< 2 × 1 卡尔曼滤波器 +using KF31f = KalmanFilter; //!< 3 × 1 卡尔曼滤波器 +using KF31d = KalmanFilter; //!< 3 × 1 卡尔曼滤波器 +using KF32f = KalmanFilter; //!< 3 × 2 卡尔曼滤波器 +using KF32d = KalmanFilter; //!< 3 × 2 卡尔曼滤波器 +using KF42f = KalmanFilter; //!< 4 × 2 卡尔曼滤波器 +using KF42d = KalmanFilter; //!< 4 × 2 卡尔曼滤波器 +using KF63f = KalmanFilter; //!< 6 × 3 卡尔曼滤波器 +using KF63d = KalmanFilter; //!< 6 × 3 卡尔曼滤波器 +using KF64f = KalmanFilter; //!< 6 × 4 卡尔曼滤波器 +using KF64d = KalmanFilter; //!< 6 × 4 卡尔曼滤波器 +using KF73f = KalmanFilter; //!< 7 × 3 卡尔曼滤波器 +using KF73d = KalmanFilter; //!< 7 × 3 卡尔曼滤波器 + +/** + * @brief 扩展卡尔曼滤波器 + * + * @tparam Tp 数据类型 + * @tparam StateDim 状态量个数 + * @tparam MeasureDim 观测量个数 + */ +template +class ExtendedKalmanFilter : public KalmanFilterStaticDatas +{ + static_assert(std::is_floating_point_v, "\"Tp\" must be floating point value."); + + cv::Matx Ja; //!< 状态方程雅可比矩阵 \f$J_A\f$ + cv::Matx Jat; //!< 状态方程雅可比矩阵的转置 \f$J_A^T\f$ + cv::Matx Jh; //!< 观测方程雅可比矩阵 \f$J_H\f$ + cv::Matx Jht; //!< 观测方程雅可比矩阵的转置 \f$J_H^T\f$ + cv::Matx W; //!< 过程噪声协方差雅可比矩阵 \f$W\f$ + cv::Matx Wt; //!< 过程噪声协方差雅可比矩阵的转置 \f$W^T\f$ + cv::Matx V; //!< 测量噪声协方差雅可比矩阵 \f$V\f$ + cv::Matx Vt; //!< 测量噪声协方差雅可比矩阵的转置 \f$V^T\f$ + + //! 非线性的离散状态方程 + std::function(const cv::Matx &)> Fa; + //! 非线性的离散观测方程 + std::function(const cv::Matx &)> Fh; + +public: + //! 构造新的 ExtendedKalmanFilter 对象 + ExtendedKalmanFilter() : KalmanFilterStaticDatas(), Ja(Ja.eye()), Jat(Jat.eye()), + Jh(Jh.eye()), Jht(Jht.eye()), W(W.eye()), Wt(Wt.eye()), V(V.eye()), Vt(Vt.eye()) {} + + /** + * @brief 设置状态方程雅可比矩阵 \f$J_A\f$ + * + * @param[in] state_jac 状态方程雅可比矩阵 */ - inline void setH(const cv::Matx &observe_tf) { H = observe_tf, Ht = observe_tf.t(); } + inline void setJa(const cv::Matx &state_jac) { Ja = state_jac, Jat = state_jac.t(); } /** - * @brief 设置测量噪声协方差矩阵 `R` + * @brief 设置观测方程雅可比矩阵 \f$J_H\f$ * - * @param[in] measure_err 测量噪声协方差矩阵 `R` + * @param[in] observe_jac 观测方程雅可比矩阵 */ - inline void setR(const cv::Matx &measure_err) { R = measure_err; } + inline void setJh(const cv::Matx &observe_jac) { Jh = observe_jac, Jht = observe_jac.t(); } /** - * @brief 设置过程噪声协方差矩阵 `Q` + * @brief 设置非线性的离散状态方程 \f$\pmb f_A(\pmb x)\f$ * - * @param[in] process_err 过程噪声协方差矩阵 `Q` + * @param[in] state_func 状态方程 */ - inline void setQ(const cv::Matx &process_err) { Q = process_err; } + inline void setFa(const std::function(const cv::Matx &)> &state_func) { Fa = state_func; } /** - * @brief 设置误差协方差矩阵 `P` + * @brief 设置非线性的离散观测方程 \f$\pmb f_H(\pmb x)\f$ * - * @param[in] state_err 误差协方差矩阵 `P` + * @param[in] observe_func 观测方程 */ - inline void setP(const cv::Matx &state_err) { P_ = P = state_err; } + inline void setFh(const std::function(const cv::Matx &)> &observe_func) { Fh = observe_func; } /** - * @brief 含系统输入的卡尔曼滤波的预测部分，包括状态向量的先验估计和误差协方差的先验估计 - * @details 预测部分公式如下 \f[\begin{align}\hat{\pmb x}^-&=A\hat{\pmb x}+B\pmb u\\P^-&=APA^T+Q\end{align}\f] + * @brief 设置过程噪声协方差雅可比矩阵 \f$W\f$ * - * @param[in] uk 系统输入向量，即公式中的 \f$\pmb u\f$ - * @return 先验状态估计 + * @see @ref ekf_state_function_linearization + * @param[in] process_jac 过程噪声协方差雅可比矩阵 */ - inline auto predict(const cv::Matx &uk) - { - u = uk; - // 估计先验状态，考虑系统输入 - x_ = A * x + B * u; - // 估计先验误差协方差 - P_ = A * P * At + Q; - return x_; - } + inline void setW(const cv::Matx &process_jac) { W = process_jac, Wt = process_jac.t();} /** - * @brief 不含系统输入的卡尔曼滤波的预测部分，包括状态量的先验估计和误差协方差的先验估计 - * @see predict(const cv::Matx &control) - * @note 公式中 \f$B\pmb u = \pmb 0\f$ + * @brief 设置测量噪声协方差雅可比矩阵 \f$V\f$ + * + * @see @ref ekf_observation_function_linearization + * @param[in] measure_jac 测量噪声协方差雅可比矩阵 + */ + inline void setV(const cv::Matx &measure_jac) { V = measure_jac, Vt = measure_jac.t();} + + /** + * @brief 扩展卡尔曼滤波的预测部分，包括状态量的先验估计和误差协方差的先验估计 + * @brief 公式如下 \f[\begin{align}\hat{\pmb x_k}^-&=\pmb f(\hat{\pmb x}_{k-1})\\ + * P_k^-&=J_AP_{k-1}J_A^T+WQW^T\end{align}\f] * * @return 先验状态估计 */ inline auto predict() { - // 估计先验状态 - x_ = A * x; - // 估计先验误差协方差 - P_ = A * P * At + Q; - return x_; + // 先验状态估计 + this->x_ = Fa(this->x); + // 先验误差协方差 + this->P_ = Ja * this->P * Jat + W * this->Q * Wt; + return this->x_; } /** - * @brief 卡尔曼滤波器校正部分，包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计 - * @details - * 更新部分公式如下 \f[\begin{align}K&=P^-H^T\left(HP^-H^T+R\right)^{-1}\\\hat{\pmb x}&= - * \hat{\pmb x}^-+K\left(\pmb z-H\hat{\pmb x}^-\right)\\P&=\left(I-KH\right)P^-\end{align}\f] + * @brief 扩展卡尔曼滤波的校正部分，包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计 + * @brief 公式如下 \f[\begin{align}K_k&=P_k^-J_H^T\left(J_HP_k^-J_H^T+VRV^T\right)^{-1}\\\hat{\pmb x} + * &=\hat{\pmb x}_k^-+K_k\left[\pmb z_k-\pmb f_H(\hat{\pmb x}_k^-)\right]\\P_k&=\left(I-K_kJ_H + * \right)P_k^-\end{align}\f] * * @param[in] zk 观测量 * @return 后验状态估计 */ inline auto correct(const cv::Matx &zk) { - z = zk; - // 计算卡尔曼增益 `K` - K = P_ * Ht * (H * P_ * Ht + R).inv(); - // 后验估计 - x = x_ + K * (z - H * x_); + this->z = zk; + // 计算卡尔曼增益 + this->K = this->P_ * Jht * (Jh * this->P_ * Jht + V * this->R * Vt).inv(); + // 后验状态估计 + this->x = this->x_ + this->K * (this->z - Fh(this->x_)); // 后验误差协方差 - P = (I - K * H) * P_; - return x; + this->P = (this->I - this->K * Jh) * this->P_; + return this->x; } }; -using KF11f = KalmanFilter; //!< 1 × 1 卡尔曼滤波器，无控制量 -using KF11d = KalmanFilter; //!< 1 × 1 卡尔曼滤波器，无控制量 -using KF22f = KalmanFilter; //!< 2 × 2 卡尔曼滤波器，无控制量 -using KF22d = KalmanFilter; //!< 2 × 2 卡尔曼滤波器，无控制量 -using KF21f = KalmanFilter; //!< 2 × 1 卡尔曼滤波器，无控制量 -using KF21d = KalmanFilter; //!< 2 × 1 卡尔曼滤波器，无控制量 -using KF33f = KalmanFilter; //!< 3 × 3 卡尔曼滤波器，无控制量 -using KF33d = KalmanFilter; //!< 3 × 3 卡尔曼滤波器，无控制量 -using KF31f = KalmanFilter; //!< 3 × 1 卡尔曼滤波器，无控制量 -using KF31d = KalmanFilter; //!< 3 × 1 卡尔曼滤波器，无控制量 -using KF32f = KalmanFilter; //!< 3 × 2 卡尔曼滤波器，无控制量 -using KF32d = KalmanFilter; //!< 3 × 2 卡尔曼滤波器，无控制量 -using KF44f = KalmanFilter; //!< 4 × 4 卡尔曼滤波器，无控制量 -using KF44d = KalmanFilter; //!< 4 × 4 卡尔曼滤波器，无控制量 -using KF42f = KalmanFilter; //!< 4 × 2 卡尔曼滤波器，无控制量 -using KF42d = KalmanFilter; //!< 4 × 2 卡尔曼滤波器，无控制量 -using KF66f = KalmanFilter; //!< 6 × 6 卡尔曼滤波器，无控制量 -using KF66d = KalmanFilter; //!< 6 × 6 卡尔曼滤波器，无控制量 -using KF63f = KalmanFilter; //!< 6 × 3 卡尔曼滤波器，无控制量 -using KF63d = KalmanFilter; //!< 6 × 3 卡尔曼滤波器，无控制量 -using KF64f = KalmanFilter; //!< 6 × 4 卡尔曼滤波器，无控制量 -using KF64d = KalmanFilter; //!< 6 × 4 卡尔曼滤波器，无控制量 +using EKF31f = ExtendedKalmanFilter; //!< 3 × 1 扩展卡尔曼滤波器 +using EKF31d = ExtendedKalmanFilter; //!< 3 × 1 扩展卡尔曼滤波器 +using EKF32f = ExtendedKalmanFilter; //!< 3 × 2 扩展卡尔曼滤波器 +using EKF32d = ExtendedKalmanFilter; //!< 3 × 2 扩展卡尔曼滤波器 +using EKF42f = ExtendedKalmanFilter; //!< 4 × 2 扩展卡尔曼滤波器 +using EKF42d = ExtendedKalmanFilter; //!< 4 × 2 扩展卡尔曼滤波器 +using EKF52f = ExtendedKalmanFilter; //!< 5 × 2 扩展卡尔曼滤波器 +using EKF52d = ExtendedKalmanFilter; //!< 5 × 2 扩展卡尔曼滤波器 +using EKF53f = ExtendedKalmanFilter; //!< 5 × 3 扩展卡尔曼滤波器 +using EKF53d = ExtendedKalmanFilter; //!< 5 × 3 扩展卡尔曼滤波器 +using EKF63f = ExtendedKalmanFilter; //!< 6 × 3 扩展卡尔曼滤波器 +using EKF63d = ExtendedKalmanFilter; //!< 6 × 3 扩展卡尔曼滤波器 +using EKF64f = ExtendedKalmanFilter; //!< 6 × 4 扩展卡尔曼滤波器 +using EKF64d = ExtendedKalmanFilter; //!< 6 × 4 扩展卡尔曼滤波器 +using EKF73f = ExtendedKalmanFilter; //!< 7 × 3 扩展卡尔曼滤波器 +using EKF73d = ExtendedKalmanFilter; //!< 7 × 3 扩展卡尔曼滤波器 +using EKF74f = ExtendedKalmanFilter; //!< 7 × 4 扩展卡尔曼滤波器 +using EKF74d = ExtendedKalmanFilter; //!< 7 × 4 扩展卡尔曼滤波器 +using EKF83f = ExtendedKalmanFilter; //!< 8 × 3 扩展卡尔曼滤波器 +using EKF83d = ExtendedKalmanFilter; //!< 8 × 3 扩展卡尔曼滤波器 +using EKF84f = ExtendedKalmanFilter; //!< 8 × 4 扩展卡尔曼滤波器 +using EKF84d = ExtendedKalmanFilter; //!< 8 × 4 扩展卡尔曼滤波器 +using EKF94f = ExtendedKalmanFilter; //!< 9 × 4 扩展卡尔曼滤波器 +using EKF94d = ExtendedKalmanFilter; //!< 9 × 4 扩展卡尔曼滤波器 //! @} kalman diff --git a/modules/core/perf/perf_kalman.cpp b/modules/core/perf/perf_kalman.cpp new file mode 100644 index 00000000..72ade041 --- /dev/null +++ b/modules/core/perf/perf_kalman.cpp @@ -0,0 +1,82 @@ +/** + * @file perf_kalman.cpp + * @author zhaoxi (535394140@qq.com) + * @brief 卡尔曼滤波器基准测试 + * @version 1.0 + * @date 2024-04-26 + * + * @copyright Copyright 2024 (c), zhaoxi + * + */ + +#include + +#include + +#include "rmvl/core/kalman.hpp" +#include + +namespace rm_test +{ + +// 4 状态量 2 观测量的 rm::KalmanFilter +void kalman42_rmvl(benchmark::State &state) +{ + for (auto _ : state) + { + rm::KalmanFilter kf; + kf.setQ(cv::Matx44f::diag({0.1, 0.1, 0.1, 0.1})); + kf.setR(cv::Matx22f::diag({1e-3f, 1e-3f})); + kf.init(cv::Matx41f::zeros(), 1e5); + cv::Matx41f xk; + std::ofstream ofs("kalman42_rmvl.txt"); + for (int i = 0; i < 1000; i++) + { + float t{0.01f}; + // 预测 + kf.setA({1, 0, t, 0, + 0, 1, 0, t, + 0, 0, 1, 0, + 0, 0, 0, 1}); + kf.predict(); + // 更新 + cv::Matx21f z(1.f * i, 2.f * i); + xk = kf.correct(z); + } + ofs << xk << std::endl; + } +} + +// 4 状态量 2 观测量的 cv::KalmanFilter +void kalman42_opencv(benchmark::State &state) +{ + for (auto _ : state) + { + cv::KalmanFilter kf(4, 2); + kf.processNoiseCov = cv::Mat::eye(4, 4, CV_32F) * 0.1; + kf.measurementNoiseCov = cv::Mat::eye(2, 2, CV_32F) * 1e-3; + kf.errorCovPost = cv::Mat::eye(4, 4, CV_32F) * 1e5; + cv::Mat xk; + std::fstream ofs("kalman42_opencv.txt"); + for (int i = 0; i < 1000; i++) + { + float t{0.01f}; + // 预测 + kf.transitionMatrix = (cv::Mat_(4, 4) << 1, 0, t, 0, + 0, 1, 0, t, + 0, 0, 1, 0, + 0, 0, 0, 1); + kf.predict(); + // 更新 + cv::Mat z = (cv::Mat_(2, 1) << 1.f * i, 2.f * i); + kf.measurementMatrix = cv::Mat::eye(2, 4, CV_32F); + xk = kf.correct(z); + } + ofs << xk << std::endl; + } +} + +BENCHMARK(kalman42_rmvl)->Name("KalmanFilter42 - RMVL")->Iterations(10); +BENCHMARK(kalman42_opencv)->Name("KalmanFilter42 - OpenCV")->Iterations(10); + +} // namespace rm_test diff --git a/modules/core/test/test_kalman.cpp b/modules/core/test/test_kalman.cpp new file mode 100644 index 00000000..04ce634b --- /dev/null +++ b/modules/core/test/test_kalman.cpp @@ -0,0 +1,150 @@ +/** + * @file test_kalman.cpp + * @author zhaoxi (535394140@qq.com) + * @brief kalman 模块单元测试 + * @version 1.0 + * @date 2024-04-18 + * + * @copyright Copyright 2024 (c), zhaoxi + * + */ + +#include + +#include "rmvl/core/kalman.hpp" + +#include + +namespace rm_test +{ + +// 一维匀速运动 KF 测试 +TEST(KalmanTest, kf) +{ + // 状态量 x = [ x v ]^T + // 状态方程 Fa = ┌ x + vt A = ┌ 1 T ┐ + // └ v └ 0 1 ┘ + // 观测量 z = x + // 观测方程 Fh = x H = [ 1 0 ] + + rm::KF21d kf; + kf.init({50, 0}, 1e5); + kf.setQ(1e-1 * cv::Matx22d::eye()); + kf.setR({1e-3}); + double t{0.01}; + + cv::Matx21d x{}; + for (int i = 0; i <= 100; i++) + { + // 预测部分，并获取先验状态估计（弃值） + kf.setA({1, t, + 0, 1}); + kf.predict(); + // 更新部分，并获取后验状态估计 + kf.setH({1, 0}); + x = kf.correct({50 + 0.3 * i}); // 以 0.3/T 的速度运动 + } + EXPECT_NEAR(x(0), 80, 1e-2); // 50 + 0.3 * 100 + EXPECT_NEAR(x(1), 30, 1e-2); // 0.3 / 0.01 +} + +// 二维匀速直线运动 KF 测试 +TEST(KalmanTest, kf2) +{ + // 状态量 x = [ x, y, u, v ]^T + // ┌ x + uT ┌ 1 0 T 0 ┐ + // 状态方程 Fa = │ y + vT A = │ 0 1 0 T │ + // │ u │ 0 0 1 0 │ + // └ v └ 0 0 0 1 ┘ + // 观测量 z = [ x, y ]^T + // 观测方程 Fh = ┌ x H = ┌ 1 0 0 0 ┐ + // └ y └ 0 1 0 0 ┘ + + rm::KF42d kf; + kf.init({50, 50, 0, 0}, 1e5); + kf.setQ(1e-1 * cv::Matx44d::eye()); + kf.setR({1e-3, 1e-3}); + double t{0.01}; + + cv::Matx41d x{}; + for (int i = 0; i <= 100; i++) + { + // 预测部分，并获取先验状态估计（弃值） + kf.setA({1, 0, t, 0, + 0, 1, 0, t, + 0, 0, 1, 0, + 0, 0, 0, 1}); + kf.predict(); + // 更新部分，并获取后验状态估计 + kf.setH({1, 0, 0, 0, + 0, 1, 0, 0}); + x = kf.correct({50 + 0.4 * i, 50 + 0.2 * i}); // 以 (0.4/T, 0.2/T) 的速度运动 + } + EXPECT_NEAR(x(0), 90, 1e-2); // 50 + 0.4 * 100 + EXPECT_NEAR(x(1), 70, 1e-2); // 50 + 0.2 * 100 + EXPECT_NEAR(x(2), 40, 1e-2); // 0.4 / 0.01 + EXPECT_NEAR(x(3), 20, 1e-2); // 0.2 / 0.01 +} + +// 二维匀速圆周运动 EKF 测试 +TEST(KalmanTest, ekf) +{ + // 状态量 x = [ cx, cy, θ, ω, r ]^T + // ┌ cx ┌ 1 0 0 0 0 ┐ + // │ cy │ 0 1 0 0 0 │ + // 状态方程 F = │ θ + ωT Ja = │ 0 0 1 T 0 │ = A + // │ ω │ 0 0 0 1 0 │ + // └ r └ 0 0 0 0 1 ┘ + // 观测量 z = [ px, py, θ ]^T + // ┌ cx + rcosθ ┌ 1 0 -rsinθ 0 cosθ ┐ + // 观测方程 H = │ cy + rsinθ Jh = │ 0 1 rcosθ 0 sinθ │ + // └ θ └ 0 0 1 0 0 ┘ + + // 正态分布噪声 + std::default_random_engine ng; + std::normal_distribution err{0, 1}; + + rm::EKF53d ekf; + ekf.init({0, 0, 0, 0, 150}, 1e5); + ekf.setQ(1e-1 * cv::Matx::eye()); + ekf.setR(cv::Matx33d::diag({1e-3, 1e-3, 1e-3})); + double t{0.01}; + ekf.setFa([=](const cv::Matx &x) -> cv::Matx { + return {x(0), + x(1), + x(2) + x(3) * t, + x(3), + x(4)}; + }); + ekf.setFh([=](const cv::Matx &x) -> cv::Matx { + return {x(0) + x(4) * std::cos(x(2)), + x(1) + x(4) * std::sin(x(2)), + x(2)}; + }); + + cv::Matx x{}; + + for (int i = 0; i <= 200; i++) + { + // 预测部分，并获取先验状态估计 + ekf.setJa({1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, t, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1}); + auto x_ = ekf.predict(); + // 更新部分，并获取后验状态估计 + ekf.setJh({1, 0, -x_(4) * std::sin(x_(2)), 0, std::cos(x_(2)), + 0, 1, x_(4) * std::cos(x_(2)), 0, std::sin(x_(2)), + 0, 0, 1, 0, 0}); + x = ekf.correct({500 + 200 * std::cos(0.02 * i) + err(ng), // 以 20 为半径，0.02/T 的角速度运动（图像上是顺时针），并加上噪声 + 500 + 200 * std::sin(0.02 * i) + err(ng), + 0.02 * i + 0.01 * err(ng)}); + } + EXPECT_NEAR(x(0), 500, 1); + EXPECT_NEAR(x(1), 500, 1); + EXPECT_NEAR(x(2), 4, 0.1); // 0.02 * 200 + EXPECT_NEAR(x(3), 2, 0.1); // 0.02 / 0.01 +} + +} // namespace rm_test diff --git a/modules/core/test/test_numcal.cpp b/modules/core/test/test_numcal.cpp index 5433b9c3..b1e195c1 100644 --- a/modules/core/test/test_numcal.cpp +++ b/modules/core/test/test_numcal.cpp @@ -37,8 +37,8 @@ TEST(NumberCalculation, function_interpolator) TEST(NumberCalculation, curve_fitter_ax_b) { rm::CurveFitter foo({1, 2, 3, 4}, {0, 2, 1, 3}, 0b11); - EXPECT_LE(abs(foo(0.625)), 1e-5); - EXPECT_LE(foo(0) + 0.5, 1e-5); + EXPECT_NEAR(foo(0.625), 0, 1e-5); + EXPECT_NEAR(foo(0), -0.5, 1e-5); } TEST(NumberCalculation, curve_fitter_ax2_bx_c) @@ -46,7 +46,7 @@ TEST(NumberCalculation, curve_fitter_ax2_bx_c) // 2x^2 + 3x - 1 rm::CurveFitter foo({0, 1, 2}, {-1, 4, 13}, 0b111); // 2*3^2 + 3*3 - 1 = 26 - EXPECT_LE(foo(3) - 26, 1e-5); + EXPECT_NEAR(foo(3), 26, 1e-5); } TEST(NumberCalculation, curve_fitter_ax3_cx_d) @@ -54,17 +54,18 @@ TEST(NumberCalculation, curve_fitter_ax3_cx_d) // x^3 - 4x + 1 rm::CurveFitter foo({0, 1, 2}, {1, -2, 1}, 0b1011); // 3^3 - 4*3 + 1 = 16 - EXPECT_LE(foo(3) - 16, 1e-5); + EXPECT_NEAR(foo(3), 16, 1e-5); } TEST(NumberCalculation, nonlinear_solver) { - rm::NonlinearSolver foo; - foo = [](double x) { return x * x - 4; }; // f(x) - EXPECT_LE(foo(2.5) - 2, 1e-5); // fo(2) = 0 - EXPECT_LE(foo(1.5) - 2, 1e-5); // fo(2) = 0 - EXPECT_LE(foo(-1.5) + 2, 1e-5); // fo(-2) = 0 - EXPECT_LE(foo(-1.5) + 2, 1e-5); // fo(-2) = 0 + rm::NonlinearSolver foo([](double x) { // f(x) + return x * x - 4; + }); + EXPECT_NEAR(foo(2.5), 2, 1e-5); // fo(2) = 0 + EXPECT_NEAR(foo(1.5), 2, 1e-5); // fo(2) = 0 + EXPECT_NEAR(foo(-1.5), -2, 1e-5); // fo(-2) = 0 + EXPECT_NEAR(foo(-1.5), -2, 1e-5); // fo(-2) = 0 } TEST(NumberCalculation, runge_kutta_ode) @@ -75,22 +76,22 @@ TEST(NumberCalculation, runge_kutta_ode) rm::RungeKutta rkb(fs, {0.0, 2.0 / 3.0}, {0.25, 0.75}, {{0.0, 0.0}, {2.0 / 3.0, 0.0}}); rkb.init(0, {0}); auto resb = rkb.solve(0.01, 100).back(); - EXPECT_LE(std::abs(resb.front() - std::expm1(-2)), 1e-4); + EXPECT_NEAR(resb.front(), std::expm1(-2), 1e-4); rm::RungeKutta2 rk2(fs); rk2.init(0, {0}); auto res2 = rk2.solve(0.01, 100).back(); - EXPECT_LE(std::abs(res2.front() - std::expm1(-2)), 1e-4); + EXPECT_NEAR(res2.front(), std::expm1(-2), 1e-4); rm::RungeKutta3 rk3(fs); rk3.init(0, {0}); auto res3 = rk3.solve(0.01, 100).back(); - EXPECT_LE(std::abs(res3.front() - std::expm1(-2)), 1e-5); + EXPECT_NEAR(res3.front(), std::expm1(-2), 1e-5); rm::RungeKutta4 rk4(fs); rk4.init(0, {0}); auto res4 = rk4.solve(0.01, 100).back(); - EXPECT_LE(std::abs(res4.front() - std::expm1(-2)), 1e-6); + EXPECT_NEAR(res4.front(), std::expm1(-2), 1e-6); } TEST(NumberCalculation, runge_kutta_odes) @@ -108,14 +109,14 @@ TEST(NumberCalculation, runge_kutta_odes) rk2.init(0, {1, -1}); auto res2 = rk2.solve(0.01, 100).back(); EXPECT_EQ(res2.size(), 2); - EXPECT_LE(std::abs(res2[0] - real_x1), 1e-4); - EXPECT_LE(std::abs(res2[1] - real_x2), 1e-4); + EXPECT_NEAR(res2[0], real_x1, 1e-4); + EXPECT_NEAR(res2[1], real_x2, 1e-4); rm::RungeKutta4 rk4(fs); rk4.init(0, {1, -1}); auto res4 = rk4.solve(0.01, 100).back(); - EXPECT_LE(std::abs(res4[0] - real_x1), 1e-6); - EXPECT_LE(std::abs(res4[1] - real_x2), 1e-6); + EXPECT_NEAR(res4[0], real_x1, 1e-6); + EXPECT_NEAR(res4[1], real_x2, 1e-6); } #if __cpp_lib_generator >= 202207L diff --git a/modules/imgproc/perf/perf_pretreat.cpp b/modules/imgproc/perf/perf_pretreat.cpp index 2c04424a..01ef6e89 100644 --- a/modules/imgproc/perf/perf_pretreat.cpp +++ b/modules/imgproc/perf/perf_pretreat.cpp @@ -65,9 +65,7 @@ void binary_brightness(benchmark::State &state) Mat src; hconcat(channel, 3, src); for (auto _ : state) - { Mat bin = binary(src, 80); - } } // threshold 方法单通道亮度二值化 @@ -87,9 +85,9 @@ void threshold_brightness(benchmark::State &state) } } -BENCHMARK(binary_BGR2Binary)->Iterations(100); -BENCHMARK(threshold_BGR2Binary)->Iterations(100); -BENCHMARK(binary_brightness)->Iterations(100); -BENCHMARK(threshold_brightness)->Iterations(100); +BENCHMARK(binary_BGR2Binary)->Name("Chns Minus - RMVL")->Iterations(20); +BENCHMARK(threshold_BGR2Binary)->Name("Chns Minus - OpenCV")->Iterations(20); +BENCHMARK(binary_brightness)->Name("Brightness - RMVL")->Iterations(20); +BENCHMARK(threshold_brightness)->Name("Brightness - OpenCV")->Iterations(20); } // namespace rm_test diff --git a/modules/light/CMakeLists.txt b/modules/light/CMakeLists.txt index dcd3ef22..cf4e5976 100644 --- a/modules/light/CMakeLists.txt +++ b/modules/light/CMakeLists.txt @@ -1,8 +1,9 @@ -find_package(OPTLightCtrl) +find_package(OPTLightCtrl QUIET) set(BUILD_rmvl_opt_light_control_INIT ${OPTLightCtrl_FOUND}) -rmvl_add_module( +rmvl_add_module(opt_light_control) + +rmvl_link_libraries( opt_light_control - EXTRA_HEADER ${OPTLightCtrl_INCLUDE_DIRS} - EXTERNAL ${OPTLightCtrl_LIBS} -) + PRIVATE optlc +) \ No newline at end of file