├── .github └── workflows │ ├── coverage.yml │ └── main.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── UR5.py ├── benchmark ├── CMakeLists.txt └── benchmark.cpp ├── demo ├── CMakeLists.txt ├── main.cpp └── robotics.hpp ├── docs ├── Chapter 0. Finite Accuracy Calculation.md ├── Chapter 0. Finite Accuracy Calculation.pdf ├── Chapter 1. Basic Linear Algebra.md ├── Chapter 1. Basic Linear Algebra.pdf ├── Chapter 4. General program design of filter.md ├── Chapter 4. General program design of filter.pdf ├── chapter1.tex ├── chapter4.tex ├── graphics │ ├── HouseHolder.png │ ├── finite_trunc_ideal_filter.png │ ├── hamming_resp.png │ ├── ideal_hamming_comp.png │ ├── ideal_lp_flt_am_G.png │ └── lin_flt_impl.png ├── img │ ├── image-20230522223735541.png │ ├── image-20230524215315175.png │ ├── image-20230524220454952.png │ ├── image-20230524235020703.png │ ├── image-20230525225025440.png │ ├── image-20230528211249065.png │ ├── image-20230529234937685.png │ ├── image-20230530000335488.png │ ├── image-20230604113620149.png │ └── image-20230604140200828.png ├── ppx Reference Manual.md ├── references.bib └── template.tex ├── include ├── asmext.hpp ├── exprtmpl.hpp ├── liegroup.hpp ├── linalg.hpp ├── matrixs.hpp ├── optimization.hpp ├── signals.hpp └── statistics.hpp ├── log ├── CMakeLists.txt ├── plog.cpp └── plog.h ├── log_server.py ├── show_trans.py ├── signals.py ├── test ├── CMakeLists.txt ├── CodeCoverage.cmake ├── lie_test.cpp ├── log_test.cpp ├── matrix_test.cpp ├── opt_test.cpp ├── signals_test.cpp ├── solver_test.cpp └── statistic_test.cpp └── tools ├── __init__.py ├── gnuplot.mplstyle └── misc.py /.github/workflows/coverage.yml: -------------------------------------------------------------------------------- 1 | name: Coverage 2 | 3 | on: 4 | workflow_dispatch: 5 | 6 | env: 7 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 8 | BUILD_TYPE: Debug 9 | 10 | jobs: 11 | build: 12 | # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. 13 | # You can convert this to a matrix build if you need cross-platform coverage. 14 | # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix 15 | runs-on: ubuntu-latest 16 | 17 | steps: 18 | - uses: actions/checkout@v3 19 | 20 | - name: install lcov 21 | #install lcov 22 | run: sudo apt install lcov 23 | 24 | - name: Configure CMake 25 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 26 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 27 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DENABLE_LCOV=ON 28 | 29 | - name: Build 30 | # Build your program with the given configuration 31 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 32 | 33 | - name: Test 34 | working-directory: ${{github.workspace}}/build 35 | # Execute tests defined by the CMake configuration. 36 | # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail 37 | run: make coverage 38 | 39 | - name: Upload coverage reports to Codecov 40 | uses: codecov/codecov-action@v3 -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | pull_request: 7 | branches: [master] 8 | 9 | jobs: 10 | build: 11 | # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. 12 | # You can convert this to a matrix build if you need cross-platform coverage. 13 | # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [windows-latest, ubuntu-latest] 18 | 19 | steps: 20 | - uses: actions/checkout@v2 21 | 22 | - name: Configure CMake 23 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 24 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 25 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=Release 26 | 27 | - name: Build 28 | # Build your program with the given configuration 29 | run: cmake --build ${{github.workspace}}/build #--config ${{env.BUILD_TYPE}} 30 | 31 | - name: Test 32 | working-directory: ${{github.workspace}}/build 33 | # Execute tests defined by the CMake configuration. 34 | # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail 35 | run: ctest -C ${{env.BUILD_TYPE}} 36 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | .idea 3 | /build 4 | /Eigen 5 | /cmake-build* 6 | /tools/__pycache__ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11) 2 | project(ppx VERSION 1.2) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_EXTENSIONS OFF) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | option(ENABLE_AVX "Enable AVX intrinsic" OFF) 9 | option(ENABLE_UNIT_TESTS "Enable unit tests" ON) 10 | option(ENABLE_BENCHMARK "Enable benchmark" OFF) 11 | option(ENABLE_LCOV "Enable code coverage tests" OFF) 12 | 13 | include(CheckCXXCompilerFlag) 14 | CHECK_CXX_COMPILER_FLAG("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE) 15 | if(COMPILER_SUPPORTS_MARCH_NATIVE) 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") 17 | endif() 18 | 19 | if(MSVC) 20 | # Force to always compile with W4 21 | if(CMAKE_CXX_FLAGS MATCHES "/W[0-4]") 22 | string(REGEX REPLACE "/W[0-4]" "/W3" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") 23 | else() 24 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /W3") 25 | endif() 26 | 27 | if(ENABLE_AVX) 28 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:AVX") 29 | endif() 30 | elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) 31 | # Update if necessary 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 33 | 34 | if(ENABLE_AVX AND COMPILER_SUPPORTS_MARCH_NATIVE) 35 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx -mfma") 36 | endif() 37 | endif() 38 | 39 | add_library(ppx INTERFACE) 40 | target_include_directories(ppx INTERFACE include) 41 | if(ENABLE_AVX) 42 | target_compile_definitions(ppx INTERFACE PPX_USE_AVX) 43 | endif() 44 | add_subdirectory(log) 45 | add_subdirectory(demo) 46 | 47 | 48 | if(ENABLE_UNIT_TESTS) 49 | enable_testing() 50 | add_subdirectory(test) 51 | endif() 52 | 53 | if(ENABLE_BENCHMARK) 54 | set(BENCHMARK_DOWNLOAD_DEPENDENCIES ON) 55 | include(FetchContent) 56 | FetchContent_Declare( 57 | benchmark 58 | GIT_REPOSITORY https://github.com/google/benchmark.git 59 | GIT_TAG v1.7.1 60 | ) 61 | FetchContent_MakeAvailable(benchmark) 62 | add_subdirectory(benchmark) 63 | endif() 64 | 65 | message(STATUS "---------------------------") 66 | message(STATUS "Current : ${PROJECT_NAME}") 67 | message(STATUS "Enable AVX-INS : ${ENABLE_AVX}") 68 | message(STATUS "Enable gtesting : ${ENABLE_UNIT_TESTS}") 69 | message(STATUS "Enable benchmark: ${ENABLE_BENCHMARK}") 70 | message(STATUS "Enable coverage : ${ENABLE_LCOV}") 71 | message(STATUS "---------------------------") -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, Xtinc 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PPX quick start 2 | 3 | [![codecov](https://codecov.io/github/Xtinc/matrix/branch/master/graph/badge.svg?token=VVGCGPHZ2W)](https://codecov.io/github/Xtinc/matrix) 4 | [![CodeFactor](https://www.codefactor.io/repository/github/xtinc/matrix/badge)](https://www.codefactor.io/repository/github/xtinc/matrix) 5 | [![CI](https://github.com/Xtinc/matrix/actions/workflows/main.yml/badge.svg)](https://github.com/Xtinc/matrix/actions/workflows/matrix.yml) 6 | 7 | A quick start for ppx users. 8 | 9 | ## Table of Contents 10 | 11 | - [Background](#background) 12 | - [Install](#install) 13 | - [Usage](#usage) 14 | - [License](#license) 15 | 16 | ## Background 17 | 18 | ppx is a cluster of algorithms for numerical calculation. It consists of 3 parts: linear algebra, non-linear optimization, statistics. 19 | 20 | The toolkit contains: 21 | 22 | 1. Linear algebra operations. ppx provides STL-like **static** containers to describe matrix and related operations such as norm-p, cofactor, determinant...(Be careful dimensions of matrix must be known before program runs. and lazy evaluation is always used in element-wised matrix operators) 23 | 24 | 2. Non-linear optimization algorithms. For 1D optimization, some line search methods are given. 25 | For N-Dimensions optimization problems, Direct method and gradient method are available. 26 | 27 | 3. Statics toolkits. currently only some filters are developed.(FIR filters generated by window function, SG smoothing filter...) 28 | 4. see documents in /doc for more! 29 | 30 | ## Install 31 | 32 | - Requires CPP standard 14 33 | - Header-only, include header file is enough 34 | 35 | ## Usage 36 | 37 | A detailed usage should refer to document in doc directory. This is an simple example for primary user. 38 | 39 | ```cpp 40 | #include "liegroup.hpp" 41 | // declarations of matrix 42 | Matrix<4, 4> a = {1, 2, 3, 4, 5, 6, 7, 8}; 43 | PRINT_SINGLE_ELEMENTS(a); 44 | PRINT_SINGLE_ELEMENTS(a(maxloc(a)), "max of a: "); 45 | // slice of a matrix 46 | a({2, 3}, {2, 3}) = Matrix<2, 2>{1, 1, 1, 1}; 47 | a(0, {0, -1}) = {89.0, 23.0, 44.0, 9.8}; 48 | PRINT_SINGLE_ELEMENTS(a, "a = "); 49 | PRINT_LISTED_ELEMENTS(a, "a in list: "); 50 | // functions for matrix operations 51 | PRINT_SINGLE_ELEMENTS(determinant(a), "determinant(a) = "); 52 | PRINT_SINGLE_ELEMENTS(inverse(a), "inverse(a) = "); 53 | // expression templates for matrix operations 54 | Matrix<4, 4> x = {1, 2, 3, 4, 5, 6, 7, 8}; 55 | Matrix<4, 4> y = x.T(); 56 | Matrix<4, 4> z = x * 2 + y * 2 + 3.3 + x * y; 57 | PRINT_SINGLE_ELEMENTS(z, "2*(x + x^T) + 3.3 +x*x^T = "); 58 | // lie group operations 59 | Matrix<3, 3> so3mat = {0, 3, -2, -3, 0, 1, 2, -1, 0}; 60 | PRINT_SINGLE_ELEMENTS(vee(so3mat).exp(), "exp(s) = "); 61 | SO3 SO3mat = {0, 1, 0, 0, 0, 1, 1, 0, 0}; 62 | PRINT_SINGLE_ELEMENTS(SO3mat.log(), "log(S) = "); 63 | PRINT_SINGLE_ELEMENTS(SE3(SO3mat, {1, 1, 1}), "T = "); 64 | SE3 SE3mat = {1, 0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0, 3, 1}; 65 | PRINT_SINGLE_ELEMENTS(SE3mat, "SE3mat = "); 66 | PRINT_SINGLE_ELEMENTS(SE3mat.log(), "log(s) = "); 67 | PRINT_SINGLE_ELEMENTS(hat(SE3mat.log())); 68 | PRINT_SINGLE_ELEMENTS(se3{1.5708, 0.0, 0.0, 0.0, 2.3562, 2.3562}.exp()); 69 | // linear solver for equations 70 | Matrix<4, 3> X{3.5, 1.6, 3.7, 4.3, 71 | 2.7, -5.7, -0.8, -9.8, 72 | -3.1, -6.0, 1.9, 6.9}; 73 | Matrix<4, 1> Y{1, 1, 1, 1}; 74 | PRINT_SINGLE_ELEMENTS(linsolve(X, Y), "solved by QR = "); 75 | PRINT_SINGLE_ELEMENTS(linsolve(X, Y), "solved by SVD = "); 76 | // factorization for matrix 77 | Matrix<4, 3> u{1, 4, 7, 11, 78 | 2, 5, 8, 1, 79 | ` 3, 6, 9, 5}; 80 | Matrix<3, 1> v{}; 81 | Matrix<3, 3> w{}; 82 | bool sing = false; 83 | u = svdcmp(u, v, w, sing); 84 | PRINT_SINGLE_ELEMENTS(u, "U = "); 85 | PRINT_SINGLE_ELEMENTS(v, "S = "); 86 | PRINT_SINGLE_ELEMENTS(w, "V = "); 87 | PRINT_SINGLE_ELEMENTS(u * v.diag() * w.T(), "U*S*V = "); 88 | // eigenvalue for symmetry matrix 89 | Matrix<4, 4> g{2, -1, 1, 4, 90 | -1, 2, -1, 1, 91 | 1, -1, 2, -2, 92 | 4, 1, -2, 3}; 93 | PRINT_SINGLE_ELEMENTS(g, "g = "); 94 | PRINT_SINGLE_ELEMENTS(eig(g).vec, "eigen vector of g : "); 95 | PRINT_SINGLE_ELEMENTS(eig(g), "eigen value of g : "); 96 | 97 | ``` 98 | 99 | ## Why a new numerical algorithm library ? 100 | 101 | When numerical algorithms come to CPP, eigen seems to be first choice for almost all of us. It's currently ultra answer for that. However, eigen now is suffering from some afflictions that all big projects suffered. For example, inconsistency in API design caused by compatibility, compromise in code constrained by generality and over abstract hierarchy. 102 | 103 | ppx is designed to be a more simple numerical toolkit library compared to that. And it's not to be as powerful, multi-functional as eigen or ceres. It makes some strict restrictions on usage to make it design and implement simple and consistent. the goal for performance of algorithm implemented in ppx is that can reach same or 10% level in time compared to eigen and ceres. 104 | 105 | generally speaking, ppx is a numerical library that has some strict restrictions but designed and coded simple for exploring and testing algorithms. 106 | 107 | 108 | ## License 109 | 110 | [MIT](LICENSE) © XTinc 111 | -------------------------------------------------------------------------------- /UR5.py: -------------------------------------------------------------------------------- 1 | """Simulated UR6 Robotics""" 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import matplotlib.animation as animation 5 | from pytransform3d.plot_utils import make_3d_axis 6 | import modern_robotics as mr 7 | from tools.misc import VisualPose 8 | from tools.misc import VisualLink 9 | 10 | M01 = np.array( 11 | [ 12 | [1.0, 0.0, 0.0, 0.0], 13 | [0.0, 1.0, 0.0, 0.0], 14 | [0.0, 0.0, 1.0, 89.159], 15 | [0.0, 0.0, 0.0, 1.0], 16 | ] 17 | ) 18 | M02 = np.array( 19 | [ 20 | [0.0, 0.0, 1.0, 0.0], 21 | [0.0, 1.0, 0.0, 135.85], 22 | [-1.0, 0.0, 0.0, 89.159], 23 | [0.0, 0.0, 0.0, 1.0], 24 | ] 25 | ) 26 | M03 = np.array( 27 | [ 28 | [0.0, 0.0, 1.0, 425.0], 29 | [0.0, 1.0, 0.0, 16.15], 30 | [-1.0, 0.0, 0.0, 89.159], 31 | [0.0, 0.0, 0.0, 1.0], 32 | ] 33 | ) 34 | M04 = np.array( 35 | [ 36 | [-1.0, 0.0, 0.0, 817.25], 37 | [0.0, 1.0, 0.0, 16.15], 38 | [0.0, 0.0, -1.0, 89.159], 39 | [0.0, 0.0, 0.0, 1.0], 40 | ] 41 | ) 42 | M05 = np.array( 43 | [ 44 | [-1.0, 0.0, 0.0, 817.25], 45 | [0.0, 1.0, 0.0, 109.15], 46 | [0.0, 0.0, -1.0, 89.159], 47 | [0.0, 0.0, 0.0, 1.0], 48 | ] 49 | ) 50 | M06 = np.array( 51 | [ 52 | [-1.0, 0.0, 0.0, 817.25], 53 | [0.0, 1.0, 0.0, 109.15], 54 | [0.0, 0.0, -1.0, -5.491], 55 | [0.0, 0.0, 0.0, 1.0], 56 | ] 57 | ) 58 | 59 | 60 | Mlist = [np.eye(4, 4), M01, M02, M03, M04, M05, M06] 61 | Slist = np.array( 62 | [ 63 | [0, 0, 0, 0, 0, 0], 64 | [0, 1, 1, 1, 0, 1], 65 | [1, 0, 0, 0, -1, 0], 66 | [0, -89.159, -89.159, -89.159, -109.15, 0.5491], 67 | [0, 0, 0, 0, 817.25, 0], 68 | [0, 0, 425, 817.25, 0, 817.25], 69 | ] 70 | ) 71 | 72 | 73 | def fk_space(thetalist): 74 | """Calculate UR6 FK all.""" 75 | result = [np.eye(4, 4)] 76 | for idx, endeffector in enumerate(Mlist): 77 | if idx != 0: 78 | result.append(mr.FKinSpace(endeffector, Slist[:, :idx], thetalist[:idx])) 79 | return result 80 | 81 | 82 | def ik_space(pose, thetalist0): 83 | """Inverse Kinematics""" 84 | return mr.IKinSpace(Slist, M06, pose, thetalist0, eomg=0.01, ev=0.001) 85 | 86 | 87 | def ur6_joint_gen(timestep): 88 | """generator of ur6 joint list""" 89 | return np.ones(6) * np.pi * 2 * timestep 90 | 91 | 92 | if __name__ == "__main__": 93 | plt.style.use("tools.gnuplot") 94 | fig = plt.figure() 95 | ax = make_3d_axis(ax_s=800) 96 | 97 | NFRAME = 200 98 | trans_list = [] 99 | for i in range(NFRAME): 100 | theta_list = ur6_joint_gen(i / NFRAME) 101 | trans_list.append(fk_space(theta_list)) 102 | 103 | transform_obj = [VisualPose(ax, t, traj_len=0, scale=0) for t in Mlist[:-1]] 104 | transform_obj.append(VisualPose(ax, Mlist[-1], traj_len=120, scale=100)) 105 | linkage_obj = [] 106 | for i in range(len(transform_obj) - 1): 107 | linkage_obj.append(VisualLink(ax, transform_obj[i], transform_obj[i + 1])) 108 | 109 | theta_list0 = np.ones(6) 110 | 111 | def update_obj(cur_f, theta0, _): 112 | """Collect all animated objs to update.""" 113 | theta, sts = ik_space(cur_f, theta0) 114 | if not sts: 115 | print("Diverge") 116 | trans_list.append(fk_space(theta)) 117 | for tend, obj in zip(trans_list[cur_f], transform_obj): 118 | obj.update(tend) 119 | for link in linkage_obj: 120 | link.update() 121 | theta0 = theta 122 | 123 | def init_obj(): 124 | """Collect all animated objs to init.""" 125 | for obj in transform_obj: 126 | obj.clear() 127 | for link in linkage_obj: 128 | link.update() 129 | 130 | anim = animation.FuncAnimation( 131 | fig, 132 | update_obj, 133 | NFRAME, 134 | init_obj, 135 | fargs=(theta_list0, NFRAME), 136 | interval=int(10000 / NFRAME), 137 | ) 138 | 139 | plt.show() 140 | -------------------------------------------------------------------------------- /benchmark/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(SUBPRJ "ppx_bench") 2 | message(STATUS "---------------------------") 3 | message(STATUS "Current : ${SUBPRJ}") 4 | add_executable(ppx_bench benchmark.cpp) 5 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") 6 | target_link_libraries(ppx_bench PRIVATE ppx pthread benchmark::benchmark) 7 | else() 8 | target_link_libraries(ppx_bench PRIVATE ppx benchmark::benchmark) 9 | endif() 10 | if(MSVC) 11 | # target_compile_options(ppx_main PRIVATE /W4) 12 | add_definitions(-D_CRT_SECURE_NO_WARNINGS) 13 | else() 14 | target_compile_options(ppx_bench PRIVATE -Wall -Wextra -pedantic) 15 | endif() -------------------------------------------------------------------------------- /benchmark/benchmark.cpp: -------------------------------------------------------------------------------- 1 | #include "benchmark/benchmark.h" 2 | #include "../demo/robotics.hpp" 3 | #include "liegroup.hpp" 4 | 5 | using namespace ppx; 6 | 7 | static void gemm_for_3M(benchmark::State &state) 8 | { 9 | MatrixS<3, 3> A; 10 | MatrixS<3, 3> B; 11 | random(A); 12 | random(B); 13 | MatrixS<3, 3> C; 14 | for (auto _ : state) 15 | { 16 | C = B * A; 17 | } 18 | benchmark::DoNotOptimize(C); 19 | } 20 | 21 | static void gemm_for_4M(benchmark::State &state) 22 | { 23 | MatrixS<4, 4> A; 24 | MatrixS<4, 4> B; 25 | random(A); 26 | random(B); 27 | MatrixS<4, 4> C; 28 | for (auto _ : state) 29 | { 30 | C = B * A; 31 | } 32 | benchmark::DoNotOptimize(C); 33 | } 34 | 35 | static void gemm_for_5M(benchmark::State &state) 36 | { 37 | MatrixS<5, 4> A; 38 | MatrixS<4, 5> B; 39 | random(A); 40 | random(B); 41 | MatrixS<5, 5> C; 42 | for (auto _ : state) 43 | { 44 | C = A * B; 45 | } 46 | benchmark::DoNotOptimize(C); 47 | } 48 | 49 | static void gemm_for_6M(benchmark::State &state) 50 | { 51 | MatrixS<6, 4> A; 52 | MatrixS<4, 6> B; 53 | random(A); 54 | random(B); 55 | MatrixS<4, 4> C; 56 | for (auto _ : state) 57 | { 58 | C = B * A; 59 | } 60 | benchmark::DoNotOptimize(C); 61 | } 62 | 63 | static void gemm_for_9M(benchmark::State &state) 64 | { 65 | MatrixS<9, 4> A; 66 | MatrixS<4, 7> B; 67 | random(A); 68 | random(B); 69 | MatrixS<9, 7> C; 70 | for (auto _ : state) 71 | { 72 | C = A * B; 73 | } 74 | benchmark::DoNotOptimize(C); 75 | } 76 | 77 | static void sum_for_5M(benchmark::State &state) 78 | { 79 | MatrixS<5, 10> A; 80 | random(A); 81 | double s{}; 82 | for (auto _ : state) 83 | { 84 | s = sum(A.data(), 50); 85 | } 86 | benchmark::DoNotOptimize(s); 87 | } 88 | 89 | static void sum_for_9M(benchmark::State &state) 90 | { 91 | MatrixS<25, 10> A; 92 | random(A); 93 | double s{}; 94 | for (auto _ : state) 95 | { 96 | s = sum(A.data(), 250); 97 | } 98 | benchmark::DoNotOptimize(s); 99 | } 100 | 101 | static void norm2_for_5M(benchmark::State &state) 102 | { 103 | MatrixS<50, 1> A; 104 | random(A); 105 | double s{}; 106 | for (auto _ : state) 107 | { 108 | s = norm2(A); 109 | } 110 | benchmark::DoNotOptimize(s); 111 | } 112 | 113 | static void norm2_for_9M(benchmark::State &state) 114 | { 115 | MatrixS<250, 1> A; 116 | random(A); 117 | double s{}; 118 | for (auto _ : state) 119 | { 120 | s = norm2(A); 121 | } 122 | benchmark::DoNotOptimize(s); 123 | } 124 | 125 | static void linsolve_LU(benchmark::State &state) 126 | { 127 | MatrixS<16, 1> b; 128 | MatrixS<16, 16> A; 129 | random(A); 130 | b.fill(1); 131 | EqnResult<16> x; 132 | for (auto _ : state) 133 | { 134 | x = linsolve(A, A * b); 135 | } 136 | benchmark::DoNotOptimize(x); 137 | } 138 | 139 | static void linsolve_QR(benchmark::State &state) 140 | { 141 | MatrixS<16, 1> b; 142 | MatrixS<16, 16> A; 143 | random(A); 144 | b.fill(1); 145 | EqnResult<16> x; 146 | for (auto _ : state) 147 | { 148 | x = linsolve(A, A * b); 149 | } 150 | benchmark::DoNotOptimize(x); 151 | } 152 | 153 | static void linsolve_SVD(benchmark::State &state) 154 | { 155 | MatrixS<16, 1> b; 156 | MatrixS<16, 16> A; 157 | random(A); 158 | b.fill(1); 159 | EqnResult<16> x; 160 | for (auto _ : state) 161 | { 162 | x = linsolve(A, A * b); 163 | } 164 | benchmark::DoNotOptimize(x); 165 | } 166 | 167 | static void codo_perf(benchmark::State &state) 168 | { 169 | kinematics<6> UR5; 170 | SE3 F6{-1.0, 0.0, 0.0, 0.0, 171 | 0.0, 0.0, 1.0, 0.0, 172 | 0.0, 1.0, 0.0, 0.0, 173 | 0.817, 0.191, -0.006, 1.0}; 174 | UR5.Joint<0>() = {"R1", se3{0, 0, 1, 0, 0, 0, 0}, SE3(), -PI, PI}; 175 | UR5.Joint<1>() = {"R2", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.0}, SE3{}, -PI, PI}; 176 | UR5.Joint<2>() = {"R3", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.425}, SE3{}, -PI, PI}; 177 | UR5.Joint<3>() = {"R4", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.817}, SE3{}, -PI, PI}; 178 | UR5.Joint<4>() = {"R5", se3{0.0, 0.0, -1.0, -0.109, 0.817, 0.0}, SE3{}, -PI, PI}; 179 | UR5.Joint<5>() = {"R6", se3{0.0, 0.0, 0.0, 0.006, 0.0, 0.817}, F6, -PI, PI}; 180 | kinematics<6>::Q q{-1.89406, 0.205429, 2.17377, -2.37914, 0.022822, 1.95129}; 181 | // kinematics<6>::Q q{2.36254, -1.97934, -0.231841, -0.581726, 1.22112, 2.1659}; 182 | 183 | SE3 TargetPose = UR5.forwardSpace(q); 184 | for (auto _ : state) 185 | { 186 | q = UR5.inverseSpace(TargetPose, q - 0.05); 187 | } 188 | benchmark::DoNotOptimize(q); 189 | } 190 | 191 | BENCHMARK(gemm_for_3M); 192 | BENCHMARK(gemm_for_4M); 193 | BENCHMARK(gemm_for_5M); 194 | BENCHMARK(gemm_for_6M); 195 | BENCHMARK(gemm_for_9M); 196 | BENCHMARK(sum_for_5M); 197 | BENCHMARK(sum_for_9M); 198 | BENCHMARK(norm2_for_5M); 199 | BENCHMARK(norm2_for_9M); 200 | BENCHMARK(linsolve_LU); 201 | BENCHMARK(linsolve_QR); 202 | BENCHMARK(linsolve_SVD); 203 | BENCHMARK(codo_perf); 204 | BENCHMARK_MAIN(); 205 | -------------------------------------------------------------------------------- /demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(SUBPRJ "demo") 2 | message(STATUS "---------------------------") 3 | message(STATUS "Current : ${SUBPRJ}") 4 | add_executable(demo main.cpp) 5 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") 6 | target_link_libraries(demo PRIVATE plog ppx pthread) 7 | else() 8 | target_link_libraries(demo PRIVATE plog ppx) 9 | endif() 10 | if(MSVC) 11 | # target_compile_options(ppx_main PRIVATE /W4) 12 | add_definitions(-D_CRT_SECURE_NO_WARNINGS) 13 | else() 14 | target_compile_options(demo PRIVATE -Wall -Wextra -pedantic) 15 | endif() -------------------------------------------------------------------------------- /demo/main.cpp: -------------------------------------------------------------------------------- 1 | #include "robotics.hpp" 2 | #include "plog.h" 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace ppx; 8 | using namespace ppx::details; 9 | 10 | #define EXPECT_EQ(A, B) assert(A == B) 11 | #define EXPECT_NEAR(A, B, C) assert(fabs(A - B) < C) 12 | 13 | template 14 | inline void PRINT_SINGLE_ELEMENTS(const T &coll, const std::string &optcsrt = "") 15 | { 16 | LOG_CH(111) << optcsrt << coll; 17 | } 18 | 19 | template 20 | inline void PRINT_LISTED_ELEMENTS(const T &coll, const std::string &optcsrt = "") 21 | { 22 | LOG_CH(111) << optcsrt << coll; 23 | } 24 | 25 | void test_expr() 26 | { 27 | MatrixS<8, 4> m; 28 | ones(m); 29 | PRINT_SINGLE_ELEMENTS(m, "m = "); 30 | // elem 0 31 | m.sub<2, 2, false>(0, 0) = {3, 3, 3}; 32 | m.sub<3, 1, false>(3, 3) = Abs(m.sub<3, 1>(3, 3) * 2 - MatrixS<3, 1>{}); 33 | PRINT_SINGLE_ELEMENTS(m, "m = "); 34 | } 35 | 36 | void test_matrix() 37 | { 38 | MatrixS<4, 4> a = {1, 2, 3, 4, 5, 6, 7, 8}; 39 | PRINT_SINGLE_ELEMENTS(a); 40 | PRINT_SINGLE_ELEMENTS(a(maxloc(a).first, maxloc(a).second), "max of a : "); 41 | a.sub<2, 2>(2, 2) = MatrixS<2, 2>{1, 1, 1, 1} + 1; 42 | a.sub<2, 2>(0, 0) = a.sub<2, 2>(1, 1); 43 | a *= -1; 44 | PRINT_SINGLE_ELEMENTS(a); 45 | PRINT_SINGLE_ELEMENTS(Abs(a).eval()); 46 | // a(0, {0, -1}) = {89.0, 23.0, 44.0, 9.8}; 47 | PRINT_SINGLE_ELEMENTS(a, "a = "); 48 | PRINT_LISTED_ELEMENTS(a, "a in list: "); 49 | PRINT_SINGLE_ELEMENTS(determinant(a), "determinant(a) = "); 50 | PRINT_SINGLE_ELEMENTS(inverse(a), "inverse(a) = "); 51 | MatrixS<4, 4> A(std::move(a)); 52 | PRINT_LISTED_ELEMENTS(a, "Move Semantics, a = "); 53 | PRINT_SINGLE_ELEMENTS(A, "Move Semantics, A = "); 54 | MatrixS<2, 2> b = {1, 2, 4, 3}; 55 | PRINT_SINGLE_ELEMENTS(inverse(b), "inverse(b) = "); 56 | MatrixS<1, 1> c = {3}; 57 | PRINT_SINGLE_ELEMENTS(c.I(), "inverse(c) = "); 58 | MatrixS<30, 30> M{}; 59 | std::default_random_engine eni; 60 | std::uniform_real_distribution<> uf(-5, 5); 61 | for (auto &ele : M) 62 | { 63 | ele = uf(eni); 64 | } 65 | PRINT_SINGLE_ELEMENTS(M, "M = "); 66 | PRINT_SINGLE_ELEMENTS(determinant(M), "determinant(M) = "); 67 | PRINT_SINGLE_ELEMENTS(adjugate(M), "adjoint(M) = "); 68 | MatrixS<4, 4> x = {1, 2, 3, 4, 5, 6, 7, 8}; 69 | MatrixS<4, 4> y = x.T(); 70 | MatrixS<4, 4> z; 71 | z.sub<4, 4>(0, 0) = x * 2 + y * 2 + 3.3 + x * y; 72 | PRINT_SINGLE_ELEMENTS(z, "2*(x + x^T) + 3.3 +x*x^T = "); 73 | MatrixS<3, 3> so3mat = {0, 3, -2, -3, 0, 1, 2, -1, 0}; 74 | PRINT_SINGLE_ELEMENTS(vee(so3mat).exp(), "exp(s) = "); 75 | SO3 SO3mat = {0, 1, 0, 0, 0, 1, 1, 0, 0}; 76 | PRINT_SINGLE_ELEMENTS(SO3mat.log(), "log(S) = "); 77 | PRINT_SINGLE_ELEMENTS(SE3(SO3mat, {1, 1, 1}), "T = "); 78 | SE3 SE3mat = {1, 0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0, 3, 1}; 79 | PRINT_SINGLE_ELEMENTS(SE3mat, "SE3mat = "); 80 | PRINT_SINGLE_ELEMENTS(SE3mat.log(), "log(s) = "); 81 | PRINT_SINGLE_ELEMENTS(hat(SE3mat.log())); 82 | PRINT_SINGLE_ELEMENTS(se3{1.5708, 0.0, 0.0, 0.0, 2.3562, 2.3562}.exp()); 83 | MatrixS<4, 3> X{3.5, 1.6, 3.7, 4.3, 84 | 2.7, -5.7, -0.8, -9.8, 85 | -3.1, -6.0, 1.9, 6.9}; 86 | MatrixS<4, 1> Y{1, 1, 1, 1}; 87 | PRINT_SINGLE_ELEMENTS(linsolve(X, Y), "solved by QR = "); 88 | PRINT_SINGLE_ELEMENTS(linsolve(X, Y), "solved by SVD = "); 89 | PRINT_SINGLE_ELEMENTS(X, " X = "); 90 | MatrixS<4, 3> u{1, 4, 7, 11, 91 | 2, 5, 8, 1, 92 | 3, 6, 9, 5}; 93 | SVD<4, 3> ru(u); 94 | PRINT_SINGLE_ELEMENTS(ru.u, "U = "); 95 | PRINT_SINGLE_ELEMENTS(ru.w, "S = "); 96 | PRINT_SINGLE_ELEMENTS(ru.v, "V = "); 97 | PRINT_SINGLE_ELEMENTS(ru.u * ru.w.diag() * ru.v.T(), "U*S*V = "); 98 | 99 | MatrixS<4, 4> g{2, -1, 1, 4, 100 | -1, 2, -1, 1, 101 | 1, -1, 2, -2, 102 | 4, 1, -2, 3}; 103 | PRINT_SINGLE_ELEMENTS(g, "g = "); 104 | EigenValue<4> eig(g); 105 | PRINT_SINGLE_ELEMENTS(eig.z, "eigen vector of g : "); 106 | PRINT_SINGLE_ELEMENTS(eig.d, "eigen value of g : "); 107 | // Matrix<3, 3> TA{1, 2, 3, 9, 8, 7, 5, 6, 4}; 108 | // Unsymmeig<3> eigsolver; 109 | // eigsolver(TA); 110 | // PRINT_LISTED_ELEMENTS(eigsolver.wri, "eigvalue: "); 111 | // PRINT_SINGLE_ELEMENTS(eigsolver.zz, "eigen_vec: "); 112 | } 113 | 114 | void test_linear() 115 | { 116 | std::default_random_engine eni((unsigned)time(NULL)); 117 | std::uniform_real_distribution<> uf(-10000, 10000); 118 | LOG_FMT(001, "Test linear linsolver LU\n"); 119 | for (size_t i = 0; i < 50; i++) 120 | { 121 | MatrixS<100, 100> A; 122 | for (auto &ele : A) 123 | { 124 | ele = uf(eni); 125 | } 126 | MatrixS<100, 1> x; 127 | for (auto &ele : x) 128 | { 129 | ele = uf(eni); 130 | } 131 | auto b = A * x; 132 | auto result = linsolve(A, b); 133 | auto residual = norm2<100, 1>(result.x - x); 134 | PRINT_SINGLE_ELEMENTS(residual, "residual = "); 135 | } 136 | LOG_FMT(001, "Test linear linsolver SVD\n"); 137 | for (size_t i = 0; i < 50; i++) 138 | { 139 | MatrixS<100, 100> A; 140 | for (auto &ele : A) 141 | { 142 | ele = uf(eni); 143 | } 144 | MatrixS<100, 1> x; 145 | for (auto &ele : x) 146 | { 147 | ele = uf(eni); 148 | } 149 | auto b = A * x; 150 | auto result = linsolve(A, b); 151 | auto residual = norm2<100, 1>(result.x - x); 152 | PRINT_SINGLE_ELEMENTS(residual, "residual = "); 153 | } 154 | LOG_FMT(001, "Test linear linsolver QR\n"); 155 | for (size_t i = 0; i < 50; i++) 156 | { 157 | MatrixS<100, 100> A; 158 | for (auto &ele : A) 159 | { 160 | ele = uf(eni); 161 | } 162 | MatrixS<100, 1> x; 163 | for (auto &ele : x) 164 | { 165 | ele = uf(eni); 166 | } 167 | auto b = A * x; 168 | auto result = linsolve(A, b); 169 | auto residual = norm2<100, 1>(result.x - x); 170 | PRINT_SINGLE_ELEMENTS(residual, "residual = "); 171 | } 172 | } 173 | 174 | void test_nonlinear() 175 | { 176 | LOG_FMT(001, "test nonlinear 1D:\n"); 177 | auto f1 = [](double x) 178 | { 179 | double y = 0.0; 180 | for (int k = -10; k < 11; k++) 181 | { 182 | y += (k + 1) * (k + 1) * cos(k * x) * exp(-1 * k * k / 2.0); 183 | } 184 | return y; 185 | }; 186 | 187 | PRINT_SINGLE_ELEMENTS(fminbnd(f1, 1.0, 3.0), "f1 by GoldenSearch: "); 188 | PRINT_SINGLE_ELEMENTS(fminbnd(f1, 1.0, 3.0), "f1 by QuadraticSearch: "); 189 | PRINT_SINGLE_ELEMENTS(fminbnd(f1, 1.0, 3.0), "f1 by BrentSearch: "); 190 | 191 | auto f2 = [](double x) 192 | { 193 | return sin(x); 194 | }; 195 | 196 | PRINT_SINGLE_ELEMENTS(fminbnd(f2, 0.0, 2.0 * PI), "f2 by GoldenSearch: "); 197 | PRINT_SINGLE_ELEMENTS(fminbnd(f2, 0.0, 2.0 * PI), "f2 by QuadraticSearch: "); 198 | PRINT_SINGLE_ELEMENTS(fminbnd(f2, 0.0, 2.0 * PI), "f2 by BrentSearch: "); 199 | 200 | auto f3 = [](double x) 201 | { 202 | return sin(x - 9.0 / 7.0); 203 | }; 204 | 205 | PRINT_SINGLE_ELEMENTS(fminbnd(f3, 1, 2.0 * PI), "f3 by GoldenSearch: "); 206 | PRINT_SINGLE_ELEMENTS(fminbnd(f3, 1, 2.0 * PI), "f3 by QuadraticSearch: "); 207 | PRINT_SINGLE_ELEMENTS(fminbnd(f3, 1, 2.0 * PI), "f3 by BrentSearch: "); 208 | 209 | LOG_FMT(001, "test nonlinear ND:\n"); 210 | auto f4 = [](const MatrixS<2, 1> &x) 211 | { 212 | return 3 * x[0] * x[0] + 2 * x[0] * x[1] + x[1] * x[1] - 4 * x[0] + 5 * x[1]; 213 | }; 214 | 215 | PRINT_SINGLE_ELEMENTS(fminunc(f4, MatrixS<2, 1>{1, 1}), "f4 by Powell: "); 216 | 217 | auto f5 = [](const MatrixS<2, 1> &x) 218 | { 219 | auto sqr = x[0] * x[0] + x[1] * x[1]; 220 | return x[0] * x[1] * exp(-sqr) + sqr / 20.0; 221 | }; 222 | 223 | PRINT_SINGLE_ELEMENTS(fminunc(f5, MatrixS<2, 1>{1, 2}), "f5 by Powell: "); 224 | 225 | auto f6 = [](const MatrixS<2, 1> &x) 226 | { 227 | return 100 * (x[1] - x[0] * x[0]) * (x[1] - x[0] * x[0]) + (1 - x[0]) * (1 - x[0]); 228 | }; 229 | 230 | auto d6 = [](const MatrixS<2, 1> &x) 231 | { 232 | MatrixS<2, 1> y; 233 | y[0] = -400 * (x[1] - x[0] * x[0]) * x[0] - 2 * (1 - x[0]); 234 | y[1] = 200 * (x[1] - x[0] * x[0]); 235 | return y; 236 | }; 237 | 238 | PRINT_SINGLE_ELEMENTS(fminunc(f6, MatrixS<2, 1>{4, -8}), "f6 by Powell: "); 239 | PRINT_SINGLE_ELEMENTS(fminunc(f6, d6, MatrixS<2, 1>{4, -8}), "f6 by GradientDescent: "); 240 | PRINT_SINGLE_ELEMENTS(fminunc(f6, d6, MatrixS<2, 1>{4, -8}), "f6 by ConjuateGradient: "); 241 | PRINT_SINGLE_ELEMENTS(fminunc(f6, d6, MatrixS<2, 1>{4, -8}), "f6 by BGFS: "); 242 | 243 | auto f7 = [](const MatrixS<2, 1> &x) 244 | { 245 | return (x[0] + 2 * x[1] - 7) * (x[0] + 2 * x[1] - 7) + (2 * x[0] + x[1] - 5) * (2 * x[0] + x[1] - 5); 246 | }; 247 | 248 | auto d7 = [](const MatrixS<2, 1> &x) 249 | { 250 | MatrixS<2, 1> y; 251 | y[0] = 10 * x[0] + 8 * x[1] - 34; 252 | y[1] = 8 * x[0] + 10 * x[1] - 38; 253 | return y; 254 | }; 255 | 256 | PRINT_SINGLE_ELEMENTS(fminunc(f7, MatrixS<2, 1>{9, 8}), "f7 by Powell: "); 257 | PRINT_SINGLE_ELEMENTS(fminunc(f7, d7, MatrixS<2, 1>{9, 8}), "f7 by GradientDescent: "); 258 | PRINT_SINGLE_ELEMENTS(fminunc(f7, d7, MatrixS<2, 1>{9, 8}), "f7 by ConjuateGradient: "); 259 | PRINT_SINGLE_ELEMENTS(fminunc(f7, d7, MatrixS<2, 1>{9, 8}), "f7 by BGFS: "); 260 | } 261 | 262 | void test_statics() 263 | { 264 | MatrixS<2, 1> mu{-2, 8}; 265 | MatrixS<2, 2> sigma{0.4, 0.0, 0.0, 1.2}; 266 | MatrixS<2, 1> x; 267 | MultiGaussianDistribution<2> d(mu, sigma); 268 | PRINT_SINGLE_ELEMENTS(d.pdf(x), "pdf of [0,0] = "); 269 | PRINT_SINGLE_ELEMENTS(d.pdf(MatrixS<2, 1>{-0.6, -0.6}), "pdf of [-0.6,-0.6] = "); 270 | std::vector> x1; 271 | MultiGaussianDistribution<2> d2(MatrixS<2, 1>{2, 1}, MatrixS<2, 2>{4, 1, 1, 4}); 272 | MultiGaussianDistribution<2> d3(MatrixS<2, 1>{6, 4}, MatrixS<2, 2>{0.25, 1.6, 2.0, 16.0}); 273 | MixedGaussianDistribution<2, 3> mg; 274 | mg.setcomp(0, d, 0.3); 275 | mg.setcomp(1, d2, 0.5); 276 | mg.setcomp(2, d3, 0.2); 277 | constexpr int ITMAX = 1000; 278 | for (int n = 0; n < ITMAX; ++n) 279 | { 280 | x1.emplace_back(mg()); 281 | } 282 | PRINT_SINGLE_ELEMENTS(avg(x1), "mean = "); 283 | // for (size_t i = 0; i < ITMAX; i++) 284 | // { 285 | // PRINT_LISTED_ELEMENTS(x1[i]); 286 | // } 287 | 288 | MixedGaussianDistribution<2, 3> mge; 289 | mge.setcomp(0, MultiGaussianDistribution<2>({1, 1}, {1, 0, 0, 1}), 0.5); 290 | mge.setcomp(1, MultiGaussianDistribution<2>({2, 2}, {1, 0, 0, 1}), 0.2); 291 | mge.setcomp(2, MultiGaussianDistribution<2>({4, 4}, {1, 0, 0, 1}), 0.3); 292 | mge.fit(x1); 293 | PRINT_SINGLE_ELEMENTS(mge); 294 | // auto e1 = mean(x1); 295 | // auto e2 = mean(x2); 296 | // auto s1 = var(x1); 297 | // auto s2 = var(x2); 298 | // PRINT_SINGLE_ELEMENTS(e1, " mean of x1 = "); 299 | // PRINT_SINGLE_ELEMENTS(s1, " var of x1 = "); 300 | // PRINT_SINGLE_ELEMENTS(e2, " mean of x2 = "); 301 | // PRINT_SINGLE_ELEMENTS(s2, " var of x2 = "); 302 | } 303 | 304 | void test_lieGroup() 305 | { 306 | { 307 | MatrixS<3, 1> vec{1, 2, 3}; 308 | SO3 expected{0, 3, -2, -3, 0, 1, 2, -1, 0}; 309 | SO3 result = hat(vec); 310 | EXPECT_EQ(result, expected); 311 | PRINT_SINGLE_ELEMENTS(result, "exp(so3) = "); 312 | } 313 | { 314 | SE3 Tinput{1, 0, 0, 0, 315 | 0, 0, 1, 0, 316 | 0, -1, 0, 0, 317 | 0, 0, 3, 1}; 318 | MatrixS<4, 4> expected{0.0, 0.0, 0.0, 0.0, 319 | 0.0, 0.0, 1.57079633, 0.0, 320 | 0.0, -1.57079633, 0.0, 0.0, 321 | 0.0, 2.35619449, 2.35619449, 0.0}; 322 | auto result = hat(Tinput.log()); 323 | EXPECT_EQ(result, expected); 324 | PRINT_SINGLE_ELEMENTS(result, "log(SE3) = "); 325 | } 326 | { 327 | SE3 T{1, 0, 0, 0, 328 | 0, 0, 1, 0, 329 | 0, -1, 0, 0, 330 | 0, 0, 3, 1}; 331 | MatrixS<6, 6> expected{1, 0, 0, 0, 3, 0, 332 | 0, 0, 1, 0, 0, 0, 333 | 0, -1, 0, 3, 0, 0, 334 | 0, 0, 0, 1, 0, 0, 335 | 0, 0, 0, 0, 0, 1, 336 | 0, 0, 0, 0, -1, 0}; 337 | auto result = T.Adt(); 338 | EXPECT_EQ(expected, result); 339 | PRINT_SINGLE_ELEMENTS(result, "SE3.Adjoint = "); 340 | } 341 | { 342 | MatrixS<4, 4> se3mat = {0.0, 0.0, 0.0, 0.0, 343 | 0.0, 0.0, 1.5708, 0.0, 344 | 0.0, -1.5708, 0.0, 0.0, 345 | 0.0, 2.3562, 2.3562, 0.0}; 346 | SE3 result{{1, 0, 0, 0, 0, 1, 0, -1, 0, 0}, {0, 0, 3}}; 347 | auto cal = vee(se3mat).exp(); 348 | PRINT_SINGLE_ELEMENTS(cal, "exp(se3) = "); 349 | for (size_t i = 0; i < 16; i++) 350 | { 351 | EXPECT_NEAR(cal[i], result[i], 1.0e-5); 352 | } 353 | } 354 | } 355 | 356 | void test_robotics() 357 | { 358 | kinematics<6> UR5; 359 | SE3 F6{-1.0, 0.0, 0.0, 0.0, 360 | 0.0, 0.0, 1.0, 0.0, 361 | 0.0, 1.0, 0.0, 0.0, 362 | 0.817, 0.191, -0.006, 1.0}; 363 | UR5.Joint<0>() = {"R1", se3{0, 0, 1, 0, 0, 0, 0}, SE3(), -PI, PI}; 364 | UR5.Joint<1>() = {"R2", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.0}, SE3{}, -PI, PI}; 365 | UR5.Joint<2>() = {"R3", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.425}, SE3{}, -PI, PI}; 366 | UR5.Joint<3>() = {"R4", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.817}, SE3{}, -PI, PI}; 367 | UR5.Joint<4>() = {"R5", se3{0.0, 0.0, -1.0, -0.109, 0.817, 0.0}, SE3{}, -PI, PI}; 368 | UR5.Joint<5>() = {"R6", se3{0.0, 0.0, 0.0, 0.006, 0.0, 0.817}, F6, -PI, PI}; 369 | kinematics<6>::Q q{0.336058, 1.69911, -0.445658, -1.25227, 0.00013744, -2.71554}; 370 | SE3 TargetPose = UR5.forwardSpace(q); 371 | auto t1 = std::chrono::system_clock::now(); 372 | for (size_t i = 0; i < 2000; i++) 373 | { 374 | q = UR5.inverseSpace(TargetPose, q - 0.05); 375 | } 376 | auto t2 = std::chrono::system_clock::now(); 377 | std::cout << std::chrono::duration_cast(t2 - t1).count() / 2000.0 << " us per iter has elapsed.\n"; 378 | PRINT_SINGLE_ELEMENTS(q, "IKSpace = "); 379 | std::cout << "singualr q:" << q << "\n"; 380 | } 381 | #include 382 | using namespace std::chrono_literals; 383 | int main(int, char **) 384 | { 385 | ppx::initialize_log(); 386 | test_expr(); 387 | test_matrix(); 388 | test_linear(); 389 | test_statics(); 390 | test_lieGroup(); 391 | test_nonlinear(); 392 | test_robotics(); 393 | 394 | return EXIT_SUCCESS; 395 | } 396 | -------------------------------------------------------------------------------- /demo/robotics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VVERY_SIMPLE_ROBOTICS_HEADER 2 | #define VVERY_SIMPLE_ROBOTICS_HEADER 3 | 4 | #include "liegroup.hpp" 5 | 6 | using namespace ppx; 7 | 8 | struct joint 9 | { 10 | std::string name{"Anon"}; 11 | std::pair range{-MAX_SP, MAX_SP}; 12 | se3 screw; 13 | SE3 pose; 14 | 15 | joint() = default; 16 | 17 | joint(const std::string &name_, const se3 &screw_, const SE3 &pose_) 18 | : name(name_), screw(screw_), pose(pose_){}; 19 | joint(const std::string &name_, const se3 &screw_, const SE3 &pose_, double lo_, double hi_) 20 | : joint(name_, screw_, pose_) 21 | { 22 | range.first = lo_; 23 | range.second = hi_; 24 | }; 25 | 26 | friend std::ostream &operator<<(std::ostream &os, const joint &self) 27 | { 28 | std::cout << "Name:\t" << self.name << "\nrange:\t" << self.range.first << " " << self.range.second 29 | << "\nscrew:\t" << self.screw << "\npose:\t" << self.pose; 30 | return os; 31 | } 32 | }; 33 | 34 | template 35 | class kinematics 36 | { 37 | private: 38 | template 39 | using idx_available_t = typename std::enable_if<(L < N), RT>::type; 40 | std::array JList; 41 | 42 | public: 43 | using Q = MatrixS; 44 | template 45 | idx_available_t Joint() 46 | { 47 | return JList[L]; 48 | } 49 | 50 | template 51 | idx_available_t Joint() const 52 | { 53 | return JList[L]; 54 | } 55 | 56 | std::array &JointList() 57 | { 58 | return JList; 59 | } 60 | 61 | const std::array &JointList() const 62 | { 63 | return JList; 64 | } 65 | 66 | SE3 forwardSpace(const std::string &jointName, const Q &jointAngle) const 67 | { 68 | auto effector_idx = std::find_if(JList.begin(), JList.end(), [&jointName](const joint &elem) 69 | { return jointName == elem.name; }); 70 | if (effector_idx == JList.end()) 71 | { 72 | return {}; 73 | } 74 | SE3 effector_pose = effector_idx->pose; 75 | for (int i = (int)std::distance(JList.begin(), effector_idx); i > -1; i--) 76 | { 77 | se3 screw = JList[i].screw * jointAngle[i]; 78 | effector_pose = screw.exp() * effector_pose; 79 | } 80 | return effector_pose; 81 | } 82 | 83 | SE3 forwardSpace(const Q &jointAngle) const 84 | { 85 | SE3 effector_pose = JList.back().pose; 86 | for (int i = N - 1; i > -1; i--) 87 | { 88 | se3 screw = JList[i].screw * jointAngle[i]; 89 | effector_pose = screw.exp() * effector_pose; 90 | } 91 | return effector_pose; 92 | } 93 | 94 | MatrixS<6, N> jacobiSpace(const Q &jointAngle) 95 | { 96 | MatrixS<6, N> Js; 97 | Js.template sub<6, 1, false>(0, 0) = JList[0].screw; 98 | SE3 T; 99 | for (int i = 1; i < (int)N; i++) 100 | { 101 | se3 screw = JList[i - 1].screw * jointAngle[i - 1]; 102 | T = T * screw.exp(); 103 | Js.template sub<6, 1, false>(0, i) = T.Adt() * JList[i].screw; 104 | } 105 | return Js; 106 | } 107 | 108 | template 109 | idx_available_t> jacobiSpace(const std::array &namelist, const Q &jointAngle) 110 | { 111 | MatrixS<6, L> Js; 112 | auto JsAll = jacobiSpace(jointAngle); 113 | for (size_t i = 0; i < L; i++) 114 | { 115 | auto iter = std::find_if(JList.begin(), JList.end(), [&namelist, i](const joint &elem) 116 | { return namelist[i] == elem.name; }); 117 | if (iter != JList.end()) 118 | { 119 | auto col_idx = std::distance(JList.begin(), iter); 120 | Js.template sub<6, 1, false>(0, i) = JsAll.template sub<6, 1>(0, col_idx); 121 | } 122 | } 123 | return Js; 124 | } 125 | 126 | Q inverseSpace(const SE3 &pose, const Q &init) 127 | { 128 | auto fx = [this, &pose](const Q &q) 129 | { 130 | auto Tsb = this->forwardSpace(q); 131 | return (Tsb * pose.I()).log(); 132 | }; 133 | 134 | auto dfx = [this, &pose](const Q &q, const se3 &) 135 | { 136 | auto Tsb = this->forwardSpace(q) * pose.I(); 137 | auto JpI = eye<6>(); 138 | JpI.sub<3, 3, false>(3, 0) = -1 * hat(Tsb.Pos()); 139 | return JpI * this->jacobiSpace(q); 140 | }; 141 | 142 | Q lower, upper; 143 | for (size_t i = 0; i < N; i++) 144 | { 145 | lower[i] = JList[i].range.first; 146 | upper[i] = JList[i].range.second; 147 | } 148 | 149 | details::CoDo codo(fx, dfx, lower, upper); 150 | auto result = codo(init); 151 | return result.s == StatusCode::CONVERGED ? result.x : init; 152 | } 153 | }; 154 | 155 | #endif -------------------------------------------------------------------------------- /docs/Chapter 0. Finite Accuracy Calculation.md: -------------------------------------------------------------------------------- 1 | # Chapter 0. Finite Accuracy Calculation 2 | 3 | 将数学公式搬运到电脑的过程中，经常会发现这些程序有非预期的表现。简单如在Python执行以下命令： 4 | 5 | ```python 6 | >>> 1+1.01-1.01 7 | 0.9999999999999998 8 | >>> 1-1.01+1.01 9 | 1.0 10 | ``` 11 | 12 | 或是使用求根公式$\frac{-b±\sqrt{b^2-4ac}}{2a}$对$x^2-24691356x-1=0$进行求根： 13 | 14 | ```python 15 | >>> x 16 | -4.097819328308106e-08（错误的） 17 | >>> x_real 18 | -4.050000332100021e-08（正确的） 19 | ``` 20 | 21 | 如此简单操作竟有意想不到的情况。为什么会出现这些错误？要如何避免它们？这些都是数值计算解决的问题。这一问题不仅发生在个人开发者中，即使最知名的企业也常被困扰：早期奔腾系列FDIV（浮点除）指令集实现具有数值问题，直接导致当年Intel损失4.75亿美元[^1]。也许数值计算不能解决一切出现在计算机数值实践中的问题，但毫无疑问：不经过严格数值训练人员编写的计算程序并不可靠。 22 | 23 | # 实数的有限精度表示 24 | 25 | 计算机系统本质上只能表示整数。对于实数及其代数运算，计算机提供的是一种称为浮点数的替代集合[^2]。但这种实数集与浮点数集之间并非一一映射，导致很多实数性质在浮点数集合中并不成立。这种使用浮点数集合以及相关运算近似实数集和其代数的方法称为计算机的有限精度表示。 26 | 27 | ## 浮点模型 28 | 29 | **浮点**（英语：floating point）是一种对于实数的近似值数值表现法，由一个有效数字（即尾数）加上幂指数来表示，通常是乘以某个基数的整数次指数得到。以这种表示法表示的数值，称为**浮点数**（floating-point number）。利用浮点进行运算，称为**浮点计算**，但这种有限精度运算常伴随着近似或舍入误差。 30 | 31 | 计算机使用浮点数运算的原因在于硬件本质上只处理整数。例如：$4÷2=2，4=100_{(2)}、2=010_{(2)}$，在二进制相当于退一位数。则$1.0÷2=0.5=0.1_{(2)}$也就是$\frac{1}{2}$。依此类推，二进制的$0.01_{(2)}$就是十进制$\frac{1}{2^2}=\frac{1}{4}=0.25$。由于十进位制无法准确换算成二进位制的部分小数，如0.1，因此只能使用有限和的近似表达。 32 | 33 | 浮点表示方法类似于基数为10的科学记数法，但在计算机上，使用2为基数的幂级数来表示。一个浮点数*a*由两个数*m*和*e*来表示：*a = m × be*。在任意一个这样的系统中，我们选择一个基数*b*（记数系统的基）和精度*p*（即使用多少位来存储）。尾数*m*是形如$±d**.**ddd...ddd$的*p*位数（每一位是一个介于0到*b*-1之间的整数，包括0和*b*-1）。如果*m*的第一位是非0整数，*m*称作**正规化**的浮点数。有一些规范使用一个单独的符号位（*s* 代表+或者-）来表示正负，这样*m*必须是正的。*e*是指数。 34 | 35 | 这些表示法的设计，来自于对于值的表现范围，与精度之间的取舍：可以在某个固定长度的存储空间内表示出某个实数的近似值。例如，一个指数范围为±4的4位十进制浮点数可以用来表示43210，4.321或0.0004321，但是没有足够的精度来表示432.123和43212.3（必须近似为432.1和43210）。 36 | 37 | ## IEEE 754标准 38 | 39 | 如前所述，浮点模型本质就是一种使用幂级数有限和近似实数的方法。但模型中对于尾数、指数、符号位的分配并无要求。每一种取舍方式都生成了新的浮点模型。为了保证实数运算在不同的硬件平台中有一致行为，电气电子工程师协会（IEEE）规定了标准化的浮点数模型IEEE 754[^3]。这一模型已被当前绝大多数硬件使用，因此后文都基于该模型进行讨论，并称为标准浮点数模型。 40 | 41 | 在标准浮点数模型中，首位是符号位*S*，然后是指数位*E*，最后是尾数位*M*，它简单的表示为： 42 | $$ 43 | S\times M\times 2^{E-e}\tag{1} 44 | $$ 45 | 其中的*e*是固定偏置。对于单精度浮点数（float）和双精度浮点数（double），其取值范围是： 46 | $$ 47 | \begin{array}{|c|c|c|c|c|} \hline & S & E & F & \text { Value } \\ \hline \text { float } & \text { any } & [1,254] & \text { any } & (-1)^{S} \times 2^{E-127} \times 1 . F \\ & \text { any } & 0 & \text { nonzero } & (-1)^{S} \times 2^{-126} \times 0 . F^{*} \\ & 0 & 0 & 0 & +0.0 \\ & 1 & 0 & 0 & -0.0 \\ & 0 & 255 & 0 & +\infty \\ & 1 & 255 & 0 & -\infty \\ & \text { any } & 255 & \text { nonzero } & \text { NaN } \\ \text { double } & \text { any } & [1,2046] & \text { any } & (-1)^{S} \times 2^{E-1023} \times 1 . F \\ & 0 & 0 & 0 & (-1)^{S} \times 2^{-1022} \times 0 . F^{*} \\ & 1 & 0 & 0 & +0.0 \\ & 0 & 2047 & 0 & -0.0 \\ & 1 & 2047 & 0 & +\infty \\ & \text { any } & 2047 & \text { nonzero } & \text { NaN } \\ \hline \end{array} \\ 48 | $$ 49 | 这种规定的结果使得浮点数在二进制与十进制下的有效数字数如下[^4]： 50 | 51 | | Type | Significant digits | Number of bytes | 52 | | :----: | :----------------: | :-------------: | 53 | | float | 6 - 7 | 4 | 54 | | double | 15 - 16 | 8 | 55 | 56 | | Type | Exponent length | Mantissa length | 57 | | :----: | :-------------: | :-------------: | 58 | | float | 8 bits | 23 bits | 59 | | double | 11 bits | 52 bits | 60 | 61 | 这也是常说的单精度浮点表示范围$[1.175494351\times 10^{- 38}, 3.402823466\times 10^{38}]$，有效数字7位；双精度浮点表示范围$[2.2250738585072014 \times 10^{- 308},1.7976931348623158\times 10^{308}]$，有效数字15位由来。 62 | 63 | 举一些简单例子帮助理解标准浮点的记法： 64 | $$ 65 | 1.0=0 01111111111 0000 (+ 48 \ more\ zeros)=+1\times 2^{1023-1023}\times (1.0)_2 \tag{2} 66 | $$ 67 | 这种记录方法可以通过以下代码验证： 68 | 69 | ```c 70 | union Udoub 71 | { 72 | double d; 73 | unsigned char c[8]; 74 | }; 75 | void main() 76 | { 77 | Udoub u; 78 | u.d = 6.5; 79 | for (int i = 7; i >= 0; i--) 80 | printf("%02x", u.c[i]); 81 | printf("\n"); 82 | } 83 | ``` 84 | 85 | 它会打出401a000000000000，如果写成二进制形式则是0100 0000 0001 1010，进一步改写： 86 | $$ 87 | 0 10000000001 1010 (+ 48\ more\ zeros) =+1 \times 2^{1025-1023} \times (1.1010)_2 = 6.5 88 | $$ 89 | 得到最初的实数6.5。 90 | 91 | 理解浮点数系统运作原理后，不难想到：利用有限项级数和近似实数，会把一个区间内所有实数映射到浮点数集的一个点上。这种区间称为浮点数分辨力。由于浮点数集在数轴上并非均匀分布，故取其最小值做为近似误差界估计$\epsilon$，该值在C++中可通过numeric _limits::epsilon()可以获得，小于该数字的两个实数在计算机中不可分辨。利用$\epsilon$ ，可以得出对两浮点数做*N*次运算带来的误差界限：$\sqrt{N}\epsilon$。 92 | 93 | ## 一些例子 94 | 95 | ### 一元二次方程求根 96 | 97 | 考虑求解一元二次方程： 98 | $$ 99 | x^{2}-2*12345678x-1=0 100 | $$ 101 | 若使用一般求根公式： 102 | $$ 103 | x_{1,2}=\frac{-b±\sqrt{b^{2}-4ac}}{2a} 104 | $$ 105 | 较小解为： 106 | $$ 107 | x_{min}=-4.097819328308106\times 10^{-8} 108 | $$ 109 | 但实际上，真正解为： 110 | $$ 111 | x_{min}=-4.050000332100021\times 10^{-8} 112 | $$ 113 | 这是因为分子中*4ac*和*b*作运算舍入误差引起的。故需要使用以下的求根公式： 114 | $$ 115 | \begin{aligned} q&=-\frac{1}{2}(b+sign(b)\sqrt{b^2-4ac})\\ x_1&=\frac{q}{a}\\ x_2&=\frac{c}{q} \end{aligned} \tag{3} 116 | $$ 117 | 该公式给出解为： 118 | $$ 119 | x_{min}=-4.050000332100025\times 10^{-8} 120 | $$ 121 | 122 | 123 | ### 浮点数减法 124 | 125 | 在本章最开头已经给出了浮点数减法的例子： 126 | 127 | ```python 128 | >>> 1+1.01-1.01 129 | 0.9999999999999998 130 | >>> 1-1.01+1.01 131 | 1.0 132 | ``` 133 | 134 | 这这浮点数相减带来的精度问题有专门的名称：**灾难性抵消**（英语：catastrophic cancellation）[^5]。发生灾难性抵消是因为减法运算对邻近数值的输入是病态的：即使近似值$\tilde {x}=x(1+\delta _{x})$和$\tilde {y}=y(1+\delta _{y})$与真实值$x$和$y$ 相比，相对误差$|\delta _{x}|=|x-{\tilde {x}|/|x|}$和$|\delta _{y}|=|y-{\tilde {y}}|/|y|$不大，近似值差的${\tilde {x}}-{\tilde {y}}$与真实值差相对误差也会与真实值差$x-y$成反比： 135 | $$ 136 | \begin{aligned} 137 | \tilde{x}-\tilde{y}&=x(1+\delta_x)-y(1+\delta_y)\\ 138 | &=x-y+(x-y)\frac{x\delta_x-y\delta_y}{x-y}\\ 139 | &=(x-y)(1+\frac{x\delta_x-y\delta_y}{x-y}) 140 | \end{aligned} 141 | $$ 142 | 因此，浮点数近似运算$\tilde{x}-\tilde{y}$与实数运算$x-y$的相对误差为： 143 | $$ 144 | \frac{x\delta_x-y\delta_y}{x-y} 145 | $$ 146 | 当输入接近时，该误差会极大增加。 147 | 148 | ### 有限差分近似 149 | 150 | 考虑如下导数的近似有限差分近似： 151 | $$ 152 | f'(x)=\lim_{h\rightarrow 0}\frac{f(x+h)-f(x)}{h} 153 | $$ 154 | 在数学上，该式随*h*缩小而收敛。但程序中，$x+h$和$x$本身都是浮点数会有舍入误差，并且差分近似微分也有截断误差。假设$x=10.3,h=0.0001$，则$f(x+h)$和$f(x)$天生带上了$\epsilon$的舍入误差，又假设这一切都是在单精度下进行的，有$\epsilon \approx 10^-7$，整个系统的舍入误差约在$\epsilon x/h\approx0.01$，是一个相当糟糕的精度。如果$f(x)$的值能准确获取，那么误差界限会限制到$e_r\approx\epsilon |f(x)/h|$水平。当$f(x)$的值不能准确获取时，选取$h$大小则是一门复杂技术：圆整误差带来的求值点不精确与差分截断误差带来的近似损失对离散步长要求是矛盾的。 155 | 156 | 考虑到一阶差分的截断误差$e_t \approx|hf^{''}(x)|$，求对应离散步长使总误差$e_r+e_t$最小： 157 | $$ 158 | h\sim \sqrt{\frac{\epsilon_ff}{f^{''}}}\approx\epsilon_f^{\frac{1}{2}}x_c 159 | $$ 160 | 其中$x_c=\sqrt{f/f^{''}}$是对函数*f*在离散点处曲率的度量，故有限精度表示下的一阶差分在步长$\sqrt{\epsilon_f}$量级中取得最好精度，若*f*满足此处一阶导数与二阶导数均在同数量级，则此时相对误差为： 161 | $$ 162 | (e_r+e_t)/|f^{'}|\sim\sqrt{\epsilon_f(ff^{''}/f^{'2})}\sim\epsilon_f^{\frac{1}{2}} 163 | $$ 164 | 同理，可推断对于二阶差分： 165 | $$ 166 | f^{'}(x)\approx\frac{f(x+h)-f(x-h)}{2h} 167 | $$ 168 | 最佳步长： 169 | $$ 170 | h\sim (\frac{\epsilon_ff}{f^{'''}})^{\frac{1}{3}}\sim \epsilon_f^{\frac{1}{3}}x_c 171 | $$ 172 | 相对误差： 173 | $$ 174 | (e_r+e_t)/|f^{'}|\sim\epsilon_f^{\frac{2}{3}}f^{\frac{2}{3}}(f^{'''})^{\frac{1}{3}})\sim\epsilon_f^{\frac{2}{3}} 175 | $$ 176 | 更多相关的技术可参考C.J.F. Ridders[^6]关于计算机中有限差分精度的分析。 177 | 178 | ## 基本数值运算接口 179 | 180 | ppx中内置一些科学计算常量以及浮点数表示参数，以方便数值分析与程序编写： 181 | 182 | | 常量名 | 数值 | 备注 | 183 | | :----: | -------------------------------------- | ------------------ | 184 | | PI | $\pi$ | 圆周率 | 185 | | EPS_SP | $1.175494351\times 10^{- 38}$ | 机器精度（单精度） | 186 | | EPS_DP | $2.2250738585072014 \times 10^{- 308}$ | 机器精度（双精度） | 187 | | MAX_SP | $3.402823466\times 10^{38}$ | 浮点上限（单精度） | 188 | | MAX_DP | $1.7976931348623158\times 10^{308}$ | 浮点上限（双精度） | 189 | 190 | 此外，ppx对基本的数值代数操作也进行封装，形式来源于Fortran/Python/Matlab等。 191 | 192 | ### SIGN函数 193 | 194 | `SIGN`函数即符号函数，最早是Fortran77内置函数[^7]，在后续的语言如Matlab和Python中，其语法发生了变化。ppx中同时提供以上两种语言风格重载。 195 | 196 | **语法** 197 | 198 | ```c++ 199 | // Fortran style 200 | auto result = SIGN(2, -1); 201 | 202 | // Matlab style 203 | auto result = SIGN(-1) * 2; 204 | ``` 205 | 206 | **函数原型** 207 | 208 | *Fortran*风格： 209 | 210 | ```c++ 211 | template 212 | T SIGN(T a, T b); 213 | ``` 214 | 215 | | 参数 | 备注 | 216 | | ---- | --------------------------------- | 217 | | a | 输入参数，数字类型 | 218 | | b | 输入参数，数字类型 | 219 | | Ret | 若$b\ge 0$，返回$|a|$，否则$-|a|$ | 220 | 221 | *Matlab*风格： 222 | 223 | ```c++ 224 | template 225 | int SIGN(T a)； 226 | ``` 227 | 228 | 229 | 230 | | 参数 | 备注 | 231 | | ---- | ----------------------------- | 232 | | a | 输入参数，数字类型 | 233 | | Ret | 若a为零返回0，a正数1，a负数-1 | 234 | 235 | ### SQR函数 236 | 237 | `SQR`函数即平方函数，出现于Visual Basic与部分Fortran方言中。接口原型来源于[Sqr 函数 (Visual Basic for Applications) | Microsoft Learn](https://learn.microsoft.com/zh-cn/office/vba/language/reference/user-interface-help/sqr-function)。 238 | 239 | **语法** 240 | 241 | ```c++ 242 | auto a = SQR(4); 243 | auto b = SQR(-1.2); 244 | ``` 245 | 246 | **函数原型** 247 | 248 | ```c++ 249 | template 250 | T SQR(T a)； 251 | ``` 252 | 253 | 254 | 255 | | 参数 | 备注 | 256 | | ---- | ------------------ | 257 | | a | 输入参数，数字类型 | 258 | | Ret | 返回输入参数平方 | 259 | 260 | ### DEG_RAD函数 261 | 262 | `DEG_RAD`函数将角度制数值转换为弧度制数值，同理，`RAD_DEG`将弧度制数值转换为角度值数值。 263 | 264 | **语法** 265 | 266 | ```c++ 267 | auto a = RAD_DEG(PI / 2); 268 | auto b = DEG_RAD(90); 269 | ``` 270 | 271 | **函数原型** 272 | 273 | ```c++ 274 | constexpr double DEG_RAD(double deg)； 275 | constexpr double RAD_DEG(double rad)； 276 | ``` 277 | 278 | 279 | 280 | | 参数 | 备注 | 281 | | ---- | ----------------------- | 282 | | a | 输入参数，数字类型 | 283 | | Ret | (弧度制/角度制)数字类型 | 284 | 285 | ### CLAMP函数 286 | 287 | `CLAMP`函数是将数值截断于特定区间的函数。在C++17以后的版本，包含头文件可以使用。ppx中CLAMP接口与std::clamp基本一致。 288 | 289 | **语法** 290 | 291 | ```c++ 292 | for (const int v : {-129, -128, -1, 0, 42, 127, 128, 255, 256}) 293 | { 294 | std::cout 295 | << std::setw(04) << v 296 | << std::setw(20) << CLAMP(v, INT8_MIN, INT8_MAX) 297 | << std::setw(21) << CLAMP(v, {0, UINT8_MAX}) << '\n'; 298 | } 299 | ``` 300 | 301 | **函数原型** 302 | 303 | ```c++ 304 | template 305 | constexpr const T &CLAMP(const T &v, const T &lo, const T &hi, Compare comp); 306 | 307 | template 308 | constexpr const T &CLAMP(const T &v, const T &lo, const T &hi); 309 | 310 | template 311 | constexpr const T &CLAMP(const T &v, const std::pair &ra); 312 | ``` 313 | 314 | 315 | 316 | 317 | | 参数 | 备注 | 318 | | ---- | ----------------------- | 319 | | v | 输入参数，数字类型 | 320 | | lo | 截断范围下限 | 321 | | hi | 截断范围上限 | 322 | | ra | 截断范围（等同[lo,hi]） | 323 | | comp | 自定义比较函数 | 324 | 325 | 326 | 327 | ## 参考文献 328 | 329 | [^1]: [1994 - Annual Report](https://www.intel.com/content/www/us/en/history/history-1994-annual-report.html). Intel. June 20, 2020 [June 20, 2020]. 330 | [^2]: Golub, G. H., & Van Loan, C. F. (2013). *Matrix computations*. JHU press. 331 | [^3]: https://zh.wikipedia.org/wiki/IEEE_754 332 | 333 | [^4]: [Type float | Microsoft Learn](https://learn.microsoft.com/en-us/cpp/c-language/type-float?view=msvc-170) 334 | 335 | [^5]:[灾难性抵消 - 维基百科，自由的百科全书 (wikipedia.org)](https://zh.wikipedia.org/zh-cn/灾难性抵消) 336 | [^6]: https://www.sciencedirect.com/science/article/pii/S0141119582800570 "C.J.F. Ridders (1982). Accurate computation of F′(x) and F′(x) F″(x). *Advances in Engineering Software*. Volume 4, Issue 2, 1982, Pages 75-76." 337 | 338 | [^7]: [SIGN (The GNU Fortran Compiler)](https://gcc.gnu.org/onlinedocs/gfortran/SIGN.html) 339 | -------------------------------------------------------------------------------- /docs/Chapter 0. Finite Accuracy Calculation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/Chapter 0. Finite Accuracy Calculation.pdf -------------------------------------------------------------------------------- /docs/Chapter 1. Basic Linear Algebra.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/Chapter 1. Basic Linear Algebra.pdf -------------------------------------------------------------------------------- /docs/Chapter 4. General program design of filter.md: -------------------------------------------------------------------------------- 1 | # Chapter 4. General program design of filter 2 | 3 | 4 | 5 | ## 信号处理与数据估计 6 | 7 | 对随机信号的时间演化进行估计是一类常见任务。通常根据可用数据因果性，这些估计有更具体名称： 8 | 9 | - 滤波（filter）: 基于系统前n时刻的历史数据（含当前）估计下一时刻数据。 10 | 11 | - 平滑（smooth）: 收集系统一段时间n内数据，根据所有数据，得到每个时刻的数据。 12 | 13 | - 预测（predict）: 基于系统前n时刻的历史数据（含当前）估计n+t刻数。 14 | 15 | 三种任务本质上同源，只是根据时间因果性进行区分。因此很多算法稍作改变就能互通：如Savizkg-Golay平滑算法被称作Savizkg-Golay滤波器；均值滤波器的与滑动平均有同样的数学表达。除有特殊情况，之后视作等同。 16 | 17 | 本章讨论统计线性滤波器通用数学模型与程序设计方法。 18 | 19 | 20 | 21 | ## 统计线性滤波器 22 | 23 | ### 通用型线性滤波器设计 24 | #### 统计线性滤波器的数学模型 25 | 26 | 随机过程的频域分析法是提取信号中周期成分然后再进行积分变换在频率域上分析的一种手段。其理论基础是随机过程本身能被模拟成各种特定频率谐波叠加的系统。在这之上操作这些谐波成分的技术称为统计线性估计/滤波/平滑。 27 | 28 | 在以上假设下，将随机过程$x_t$输入以下差分系统： 29 | $$ 30 | y_{n}=\sum_{k=0}^{M} c_{k} x_{n-k}+\sum_{j=0}^{N-1} d_{j} y_{n-j-1} \tag{1} 31 | $$ 32 | 其中$y_n$是在系统对应离散输入$x_t$的输出。通过给输入与输出设定不同的系数，就能组合出特定性质的$y_t$。当等式右边项$y$相关系数都是零时，系统只与过去时刻输入有关，具有有限项、因果性、绝对稳定性，此时称为有限递归滤波器（FIR）；当右边项$y$相关系数不为零，系统输出与过去时刻输出有关，具有条件稳定性、因果性。相比FIR系统增加了反馈环节，等同于加入了无穷多项输入，称作无穷递归滤波器（IIR）[^1]。所有的线性滤波器都是这两类滤波器的特例。 33 | 34 | 该差分系统的频率域响应： 35 | $$ 36 | H(f)=\frac{\sum_{k=0}^{M} c_{k} e^{-2 \pi i kf }}{1-\sum_{j=0}^{N-1} d_{j} e^{-2 \pi i(j+1)f }} \tag{2} 37 | $$ 38 | 其中$f$是使用采用频率进行归一化后的频率。改变系数能相应改变频率响应函数形状，因此通过这种方式设计的滤波器称作频率成形滤波器[^2]。 39 | 40 | 特别注意在scipy.signal等软件包中，会限制$-0.5\le f\le0.5$[^3]。因为式2通过Laplace变换连续时间域得到系统传递函数，但实际上采样是离散过程，需要使用z变换到离散频率域。当离散系统采样频率$f_s\le 2f$时，Laplace变换与z变换转换中存在混叠效应。因此这些软件包中会限制一半的频率使用，如$f_s=8000Hz$，那么滤波器频率设定范围只有$4000Hz$，这种频率称为Nyquist频率。 41 | 42 | #### 统计线性滤波器的程序设计 43 | 44 | 线性统计滤波器的程序设计是围绕式1进行。式1是两项卷积之和，每项卷积有如下加乘累计结构： 45 | 46 | ![image-20230524235020703](img/image-20230524235020703.png) 47 | 48 | 这种置换卷积对应于固定位置索引与先进先出（FIFO）的队列元素的乘积和。将滤波器系数置入固定数组，然后将队列元素依次加成就得到滤波结果。 49 | 50 | #### 用户接口 51 | 52 | `Filter`类是ppx实现的一般统计线性滤波器设计与使用接口，定义对应有理传递函数有如下形式： 53 | $$ 54 | Y(z)=\frac{b(1)+b(2) z^{-1}+\ldots+b\left(n_{b}+1\right) z^{-n_b}}{1+a(2) z^{-1}+\ldots+a\left(n_{a}+1\right) z^{-n_{a}}} X(z) 55 | $$ 56 | 这种形式能同时处理FIR与IIR型滤波器，其中$n_a$是反馈滤波器阶数，$n_b$是前馈滤波器阶数。由归一化设定$a(1)=1$。还可以将有理传递函数表示为以下差分方程： 57 | $$ 58 | a(1)y(n)=b(1)x(n)+b(2)x(n-1)+\cdots +b(n_b+1)x(n-n_b)-a(2)y(n-1)-\cdots -a(n_a+1)y(n-n_a) 59 | $$ 60 | 61 | **功能** 62 | 63 | | 构造方法 | | 64 | | ------------ | ----------------------------------------- | 65 | | constructor | 滤波器类型：低通（默认值）/高通/带通/带阻 | 66 | | **成员方法** | | 67 | | coff_a | 返回反馈滤波器系数 | 68 | | coff_b | 返回前馈滤波器系数 | 69 | | reset | 重置滤波器状态 | 70 | | operator() | 滤波操作 | 71 | 72 | **示例** 73 | 74 | 使用有理传递函数$H(z)=\frac{1}{1-0.9z^{-1}}$对数据进行滤波。 75 | 76 | 创建由正弦谐波与白噪声组合的输入数据： 77 | 78 | ```c++ 79 | std::vector datas1; 80 | MultiNormalDistribution<1> rand(0 , 0.1); 81 | for (int i = 0; i < 100; ++i) 82 | { 83 | double sample_point = -PI + (double)i / 50 * PI; 84 | datas1.emplace_back(sin(sample_point) + rand()); 85 | } 86 | ``` 87 | 88 | 创建统计线性滤波器，并设定有理传递函数的分子和分母系数： 89 | 90 | ```c++ 91 | Filter flt; 92 | flt.coff_a() = {0.1}; 93 | flt.coff_b() = {1.0, -0.9}; 94 | ``` 95 | 96 | 对生成数据进行滤波： 97 | 98 | ```c++ 99 | for (auto x : datas1) 100 | { 101 | auto y = flt(x); 102 | } 103 | ``` 104 | 105 | image-20230604113620149 106 | 107 | 108 | 109 | ### FIR（Finite Impulse Response）型滤波器设计 110 | 111 | #### 理想有限脉冲响应滤波器 112 | 113 | FIR型滤波器系统简单表达为： 114 | $$ 115 | y_{n}=\sum_{k=0}^{M} c_{k} x_{n-k} \tag{3} 116 | $$ 117 | 118 | 对应频率响应为： 119 | $$ 120 | H(f)=\sum_{k=-\infin}^{\infin} h_{k} e^{-j2 \pi kf } 121 | $$ 122 | 这是FFT-IFFT变换对。通过改变对应谐波$\Omega=2\pi f$响应系数$h_k$得到时域上不同信号形状： 123 | $$ 124 | H(\Omega)=\sum_{k=-\infty}^{\infty} h[n] \cdot e^{-j k \Omega}\\ 125 | h[n]=\frac{1}{2 \pi} \int_{-\pi}^{\pi} H(\Omega) \cdot e^{j n \Omega} d \Omega 126 | \tag{4} 127 | $$ 128 | 故设计FIR滤波器可以直接将频率响应进行反积分变换得到系数$h[n]$。 129 | 130 | 以设计截止频率为$\Omega_c$的理想低通滤波器为例，其频率响应为： 131 | 132 | ![image-20230522223735541](C:\Users\xtc\Desktop\matrix\docs\img\image-20230522223735541.png) 133 | 134 | 即： 135 | $$ 136 | H(\Omega)=\begin{cases} 1\quad(-\Omega_c\le\Omega\le\Omega_c)\\ 0\quad(else) \end{cases} 137 | \tag{5} 138 | $$ 139 | 作IFFT： 140 | $$ 141 | h[n]=\frac{1}{2\pi}\int_{-\Omega_c}^{\Omega_c}{1\cdot e^{jn\Omega}d\Omega}=\frac{1}{2\pi}[\frac{e^{jn\Omega}}{jn}]_{-\Omega_c}^{\Omega_c}=\frac{\Omega_{c}}{\pi} \cdot \frac{\sin \left(n \Omega_{c}\right)}{\left(n \Omega_{c}\right)}=2 f_{c} \cdot \frac{\sin \left(n \Omega_{c}\right)}{\left(n \Omega_{c}\right)} 142 | \tag{6} 143 | $$ 144 | 将式6回代入式3，得理想低通滤波器时域表达式。对该无穷级数求部分和做近似。式6是偶函数，选取中心取左右$M$点，共$N=2M+1$阶作为级数部分和。考虑因果性，将负周期前移至正半周期： 145 | $$ 146 | H(z)=\sum_{n=-M}^{M} h[n] \cdot z^{-(n+M)} 147 | \tag{7} 148 | $$ 149 | 以上平移与对称性表示： 150 | 151 | 一、滤波器某时刻值与该点前和该点后数据相关，即N时刻值是由N-M和N+M这些过去与未来的数据推断，这正是1.1节中所描述的平滑任务。 152 | 153 | 二、考虑到滤波任务的因果性，我们将信号整体平移M周期，即在频率域中乘以$z^{-M}$，获得$[0,2M]$时刻数据才推断M时刻数据。这带来了相位的扭曲：对于信号$y=x(t-t_0)$，相当于信号$y=x(t)$通过$H(jw)=e^{-jwt}$环节，它使系统的所有谐波成分整体相位偏移对应$t_0$周期。这种对每个谐波成分的相位扭曲称作群延迟，是滤波器相位特性的来源。 154 | 155 | 式6给出理想低通滤波器环节的反变换。相应的，理想高通、带通、带阻对应时域表达式： 156 | 157 | | 类型 | $h[n],n\ne0$ | $h[n],n=0$ | 158 | | ---- | ------------------------------------------------------------ | ----------- | 159 | | 低通 | $2f_c\frac{sin(n\Omega_c)}{n\Omega_c}$ | $2f_c$ | 160 | | 高通 | $-2f_c\frac{sin(n\Omega_c)}{n\Omega_c}$ | $1-2f_c$ | 161 | | 带通 | $2f_2\frac{sin(n\Omega_2)}{n\Omega_2}-2f_1\frac{sin(n\Omega_1)}{n\Omega_1}$ | $2f_2-2f_1$ | 162 | | 带阻 | $2f_2\frac{sin(n\Omega_2)}{n\Omega_2}+2f_1\frac{sin(n\Omega_1)}{n\Omega_1}$ | $2f_2+2f_1$ | 163 | 164 | #### 窗函数法 165 | 166 | 对式7进行有限截断导出的滤波器频域特性和理想滤波器存在区别，主要是频响有相当明显波纹： 167 | 168 | ![image-20230524215315175](img/image-20230524215315175.png) 169 | 170 | 为了去除波纹，需要对频谱使用一些函数进行进行二次加工，这些函数称作窗函数。这些窗函数能较好改变通带与阻带特性，与理想滤波器一起构成了实际使用的FIR滤波器。以三角窗（又称bartlett窗）为例，其时域上是一个三角形，而频域上则对主瓣以外成分有强烈衰减： 171 | 172 | ![image-20230524220454952](img/image-20230524220454952.png) 173 | 174 | 常用窗函数的时域表达式如下： 175 | 176 | | 类型 | $w(n)$ | 177 | | ----------- | ------------------------------------------------------------ | 178 | | Rectangular | 1 | 179 | | Hanning | $0.5-0.5\cos(\frac{2\pi n}{N-1})$ | 180 | | Hamming | $0.54-0.46\cos(\frac{2\pi n}{N-1})$ | 181 | | Blackman | $0.42-0.5 \cos \left(\frac{2 \pi n}{N-1}\right)+0.08 \cos \left(\frac{2 \pi n}{N-1}\right)$ | 182 | 183 | 将窗函数叠加到理想滤波器系数，就得到最终滤波器系数$h[n]*w[n]$。 184 | 185 | #### 频幅响应与相位偏移 186 | 187 | 本节给出FIR的频幅响应与相位偏移。根据式4，其传递函数为： 188 | $$ 189 | H(j \omega)=\sum_{n=-M}^{M} h[n] \cdot e^{-j \omega T(n+M)}=e^{-j \omega T M} \sum_{n=-M}^{M} h[n] \cdot e^{-j \omega T_{n}} 190 | \tag{7} 191 | $$ 192 | 其中$T$是采样周期。假设系数是对称分布的，有： 193 | $$ 194 | \begin{aligned} 195 | H(j \omega) & =e^{-j \omega T M}\left\{h(0)+\sum_{n=1}^{M} h[n] \cdot\left(e^{j \omega T n}+e^{-j \omega T n}\right)\right\} \\ 196 | & =e^{-j \omega T M}\left\{h(0)+\sum_{n=1}^{M} h[n] \cdot 2 \cos (\omega T n)\right\} 197 | \end{aligned} 198 | \tag{8} 199 | $$ 200 | 式8表明，该系统对对频率$w$谐波成分会产生$-wTM$线性相位偏移，此斜率即群延迟。FIR滤波器有着线性相位偏移，即对任何频率谐波成分，有相同群延迟，对应于整个系统输入延迟$TM$时间。 201 | 202 | #### FIR型滤波器程序实现 203 | 204 | FIR型滤波器整体沿用通用线性滤波器框架，特性在于如何根据低通滤波器系数计算高通、带通、带阻滤波器相应脉冲响应系数$h[n]$。 205 | 206 | 假设现已根据窗函数法求出截止频率$\Omega_c$低通滤波器频率响应$H_{lp}(f)$，相应高通滤波器系数$H_hp(f)$与之存在对应关系[^4]： 207 | $$ 208 | \begin{aligned} 209 | H_{lp}(f)+H_{hp}(f)&=1\\ 210 | h_{lp}[n]+h_{hp}[n]&=\delta[n] 211 | \end{aligned} 212 | \tag{9} 213 | $$ 214 | 即将低通滤波系数符号取相反，中心项加一得对应高通滤波器： 215 | 216 | ```c++ 217 | h[n] = n == 0 ? 1 - h[n] : -h[n]; 218 | ``` 219 | 220 | 当然，考虑到连续域积分变换与离散域积分变换区别，式9对应实际频率响应函数为： 221 | $$ 222 | H_{h p}(f)+H_{l p}(f)=e^{-j 2 \pi f \frac{N}{2}} 223 | $$ 224 | 但这种数值误差一般不在考虑之列。 225 | 226 | 通过低通与高通滤波器也可以生成带通和带阻滤波器[^5]。带通滤波器的系数是两滤波器系数卷积；带阻滤波器系数是两滤波器之和。当然，带通滤波器也可以通过将两个低通滤波器系数相加归一化得到。 227 | 228 | image-20230525225025440 229 | 230 | #### 用户接口 231 | 232 | `FIRFilter`类是ppx实现的有限递归滤波器设计与使用接口，它有如下的构造形式： 233 | 234 | ```c++ 235 | FIRFilter(double fcf) 236 | FIRFilter(double fcf) 237 | FIRFilter(double f1f , double f2f) 238 | FIRFilter(double f1f , double f2f) 239 | ``` 240 | 241 | `FIRFilter` 中第一个模板参数代表滤波器阶数$n+1$。对于高通和带阻滤波器，这个数必须是奇数。 因为只有奇数阶FIR滤波器才会在Nyquist频率上有零增益。 242 | 243 | `FIRFilter` 中第二个模板参数代表滤波器类型。对于低通与高通滤波器，构造函数只允许传入单个浮点数作为截止频率；对于带通和带阻滤波器，构造函数必须传入两个浮点数作为截止频率。截止频率fcf使用归一化Nyquist频率。该频率只有采样频率一半，或以角频率计量只有 $π/sample$。 244 | 245 | `FIRFilter` 构造函数还有默认窗函数参数，通过 FIRType进行设定。目前支持矩形窗（无窗）、三角窗、Blackman窗、Hamming窗（默认）、Hanning窗等。设定不同窗函数可改变FIR滤波器频谱泄露特性。 246 | 247 | **功能** 248 | 249 | | 构造方法 | | 250 | | ------------ | ---------------------------------------------- | 251 | | constructor | 截止频率：fcf（低通/高通）f1f,f2f（带通/带阻） | 252 | | **成员方法** | | 253 | | coff_a | 返回反馈滤波器系数 | 254 | | coff_b | 返回前馈滤波器系数 | 255 | | reset | 重置滤波器状态 | 256 | | operator() | 滤波操作 | 257 | 258 | **示例** 259 | 260 | 设计4阶Hamming窗低通滤波器。 261 | 262 | 创建FIR滤波器，并设定阶数、类型、截止频率： 263 | 264 | ```c++ 265 | FIRFilter<3, FreqProperty::LowPass> flt(0.1); 266 | ``` 267 | 268 | 输出滤波器前馈系数，与Scipy和matlab对比： 269 | 270 | ```c++ 271 | for (auto x : flt.coff_b()) 272 | { 273 | std::cout << x << std::endl; 274 | } 275 | ``` 276 | 277 | ```bash 278 | 0.0679902 279 | 0.86402 280 | 0.0679902 281 | ``` 282 | 283 | ```matlab 284 | fir1(2,0.1) 285 | 286 | ans = 287 | 288 | 0.0680 0.8640 0.0680 289 | ``` 290 | 291 | 设计50阶Hamming窗带阻滤波器。 292 | 293 | ```c++ 294 | FIRFilter<51, FreqProperty::BandStop> flt(0.21,0.4); 295 | ``` 296 | 297 | 对生成数据进行滤波： 298 | 299 | ```c++ 300 | for (auto x : datas1) 301 | { 302 | auto y = flt(x); 303 | } 304 | ``` 305 | 306 | 根据前馈系数求出幅值响应： 307 | 308 | ![image-20230604140200828](img/image-20230604140200828.png) 309 | 310 | 311 | 312 | ### IIR（Infinite Impulse Response）型滤波器设计 313 | 314 | IIR滤波器引入了反馈作用，在实现相同的频谱特性时阶数一半远小于FIR滤波器。因此经典IIR滤波器比FIR滤波器更常见。IIR滤波器缺陷在于如不精心设计式2中的零极点在复平面分布，可能出现滤波发散情况。因此大家都使用数学结构类似的巴特沃斯滤波器（Butterworth）滤波器、切比雪夫（Chebyshev）滤波器等。 315 | 316 | #### 巴特沃斯型滤波器设计 317 | 318 | 以低通巴特沃斯滤波器设计为例，核心思想还是构造频率特性接近理想低通滤波器的传递函数，然后通过反积分变换得时域表达式。巴特沃斯滤波器选取的频域函数是这样一组函数： 319 | $$ 320 | |H_n(jw)|^{2}=\frac{1}{1+w^{2n}}\tag{10} 321 | $$ 322 | 它的归一化频率响应具有以下特性： 323 | 324 | ![image-20230528211249065](img/image-20230528211249065.png) 325 | 326 | 首先它是一个全极点滤波器，然后它通过很少阶数就达到了较好的设计频率特性。为保证稳定性求出式10处于复平面负半部分的极点： 327 | $$ 328 | p_k=-\sin\frac{\pi(2k+1)}{2n}+j\cos\frac{\pi(2k+1)}{2n}\quad k=0,1,2,\cdots,n-1 329 | \tag{11} 330 | $$ 331 | 这样在Laplace变换下： 332 | $$ 333 | H_{n}(s)=\prod_{k=0}^{n}{\frac{1}{(s-p_k)}} 334 | $$ 335 | 如1~5阶巴特沃斯滤波器频域表达式： 336 | $$ 337 | \begin{aligned} 338 | G_{1}(s)&=\frac{1}{s+1}\\ 339 | G_{2}(s)&=\frac{1}{s^{2}+\sqrt{2} s+1}\\ 340 | G_{3}(s)& =\frac{1}{s^{3}+2 s^{2}+2 s+1}\\ 341 | G_4(s)&=\frac{1}{s^{4}+\sqrt{4+\sqrt{8}} s^{3}+(2+\sqrt{2}) s^{2}+\sqrt{4+\sqrt{8}} s+1}\\ 342 | G_5(s)&=\frac{1}{(s+1)(s^4+\sqrt{5}s^3+3s^2+\sqrt{5}s+1)} 343 | \end{aligned} 344 | $$ 345 | 极点的分布是均匀对称于复平面，因此有简化表达式： 346 | $$ 347 | \begin{array}{l} 348 | H_{n}(s)=\frac{1}{D_{n}(s)} \quad n=1,2,3 \ldots \\ 349 | D_{n}(s)=\left\{\begin{array}{l} 350 | (s+1) \prod_{k=0}^{(n-3) / 2}\left(s^{2}+2 r_{k} s+1\right) \quad n=1,3,5, \ldots \\ 351 | \prod_{k=0}^{(n-2) / 2}\left(s^{2}+2 r_{k} s+1\right) \quad n=2,4,6, \ldots 352 | \end{array}\right. \\ 353 | r_{k}=\sin \left(\frac{\pi(2 k+1)}{2 n}\right) 354 | \end{array} 355 | \tag{12} 356 | $$ 357 | 358 | 式12处于连续域中，需要使用双线性变换至离散域。因此引入双线性变换[^6]：它是一种保形变换，将Laplace域变换至z域。对于截止频率$\omega_0$，采样周期$T$的低通滤波器，这种变换写作： 359 | $$ 360 | \begin{aligned} 361 | s\rightarrow&\frac{1}{a}\frac{z-1}{z+1}\\ 362 | a=&\tan(\frac{\omega_0T}{2}) 363 | \end{aligned} 364 | \tag{13} 365 | $$ 366 | 该情况下，1阶与2阶巴特沃斯滤波器频域表达式如下： 367 | $$ 368 | \begin{aligned} 369 | \frac{1}{s+1} \rightarrow& \frac{a(z+1)}{(1+a) z-(1-a)}\\ 370 | \frac{1}{s^{2}+2 r s+1} \rightarrow& \frac{a^{2}(z+1)^{2}}{\left(a^{2}+2 a r+1\right) z^{2}-2\left(1-a^{2}\right) z+\left(a^{2}-2 a r+1\right)} 371 | \end{aligned} 372 | $$ 373 | 然后对这些式子进行反z变换就得到时域表达式。值得注意，对于线性时不变系统，式1反z变换有简单方法，把式1改写成如下形式： 374 | $$ 375 | \sum_{k=0}^{M} c_{k} x_{n-k}=\sum_{k=0}^{N}e_jy_{n-j} 376 | $$ 377 | 同时取反z变换，并作时移： 378 | $$ 379 | \begin{aligned} 380 | \sum_{k=0}^{M} c_{k} z^{-k}X(z)&=\sum_{k=0}^{N}e_jz^{-j}Y(z)\\ 381 | H(z)&=\frac{\sum_{k=0}^{N}e_jz^{-j}}{\sum_{k=0}^{M} c_{k} z^{-k}} 382 | \end{aligned} 383 | \tag{14} 384 | $$ 385 | 式14系数与式12经双线性变换后系数一致，同时式14中系数即为IIR滤波器设计时域系数[^7]。巴特沃斯滤波器设计中，高通、带通、带阻滤波器区别仅在于双线性变换式13表达式不同。 386 | 387 | #### 频幅响应与相位偏移 388 | 389 | 巴特沃斯滤波器相位特性是非线性，因此只以6阶低通（截止频率$f_c=0.2f_s$）巴特沃斯滤波器为例说明其幅相特性： 390 | 391 | ![image-20230530000335488](img/image-20230530000335488.png) 392 | 393 | ![image-20230529234937685](img/image-20230529234937685.png) 394 | 395 | 该滤波器在通频带内有较均匀幅值增益，对频率为$0.2f_s$信号出现最大群延迟11周期，平均相偏约8周期。 396 | 397 | #### 用户接口 398 | 399 | `IIRFilter`类是ppx实现的无限递归滤波器设计与使用接口，它有如下的构造形式： 400 | 401 | ```c++ 402 | IIRFilter(double fcf) 403 | IIRFilter(double fcf) 404 | IIRFilter(double f1f , double f2f) 405 | IIRFilter(double f1f , double f2f) 406 | ``` 407 | 408 | `IIRFilter` 中第一个模板参数代表滤波器阶数$n$。对于带通和带阻滤波器，实际系数个数是其两倍。 409 | 410 | `IIRFilter` 中第二个模板参数代表滤波器类型。对于低通与高通滤波器，构造函数只允许传入单个浮点数作为截止频率；对于带通和带阻滤波器，构造函数必须传入两个浮点数作为截止频率。截止频率fcf使用归一化Nyquist频率。该频率只有采样频率一半，或以角频率计量只有 $π/sample$。 411 | 412 | `IIRFilter` 目前只支持ButterWorth型滤波器设计。 413 | 414 | **功能** 415 | 416 | | 构造方法 | | 417 | | ------------ | ---------------------------------------------- | 418 | | constructor | 截止频率：fcf（低通/高通）f1f,f2f（带通/带阻） | 419 | | **成员方法** | | 420 | | coff_a | 返回反馈滤波器系数 | 421 | | coff_b | 返回前馈滤波器系数 | 422 | | reset | 重置滤波器状态 | 423 | | operator() | 滤波操作 | 424 | 425 | **示例** 426 | 427 | 设计6阶ButterWorth低通滤波器。 428 | 429 | 创建IIR滤波器，并设定阶数、类型、截止频率： 430 | 431 | ```c++ 432 | IIRFilter<6, FreqProperty::LowPass> flt(0.1); 433 | ``` 434 | 435 | 输出滤波器前馈系数，与Scipy和matlab对比： 436 | 437 | ```c++ 438 | for (auto x : flt.coff_b()) 439 | { 440 | std::cout << x << std::endl; 441 | } 442 | ``` 443 | 444 | ```bash 445 | b: 446 | 8.57656e-06 5.14593e-05 0.000128648 0.000171531 0.000128648 5.14593e-05 8.57656e-06 447 | a: 448 | 1 -4.78714 9.64952 -10.4691 6.44111 -2.12904 0.295172 449 | ``` 450 | 451 | ```matlab 452 | [b,a]=butter(6,0.1) 453 | 454 | b = 455 | 456 | 1.0e-03 * 457 | 458 | 0.0086 0.0515 0.1286 0.1715 0.1286 0.0515 0.0086 459 | 460 | 461 | a = 462 | 463 | 1.0000 -4.7871 9.6495 -10.4691 6.4411 -2.1290 0.2952 464 | ``` 465 | 466 | 467 | 468 | ### 滑动均值型滤波器设计 469 | 470 | 滑动均值滤波器本质上是一种FIR滤波器[^2]，因此程序设计中继承FIR滤波器，并将其系数设置为$1/N$。 471 | 472 | 滑动均值滤波器的时域特性与频域特性描述为： 473 | $$ 474 | \begin{aligned} 475 | y[n]&=\frac{1}{N+M+1}\sum_{k=-N}^{M}x[n-k]\\ 476 | H(jw)&=\frac{1}{N+M+1}e^{jw[(N-M)/2]}\frac{\sin[w(M+N+1)/2]}{\sin(w/2)} 477 | \end{aligned} 478 | \tag{15} 479 | $$ 480 | 滑动均值滤波器具有固定的相位迟滞（群延迟）$(N-M)/2$，同时，正系数具有低通的幅值响应特性，负系数具有高通的幅值响应特性。以三点均值滤波为例： 481 | $$ 482 | y[n]=\frac{1}{3}[x[n-1]+x[n]+x[n+1]] 483 | $$ 484 | 每个$y[n]$都是三个连续输入值的平均，这时： 485 | $$ 486 | h[n]=\frac{1}{3}[\delta[n-1]+\delta[n]+\delta[n+1]] 487 | $$ 488 | 相应的频率响应为： 489 | $$ 490 | H(jw)=\frac{1}{3}[e^{jw}+1+e^{-jw}]=\frac{1}{3}(1+2\cos w) 491 | $$ 492 | 493 | #### 用户接口 494 | 495 | `MovAvgFilter`类是ppx实现的滑动均值滤波器使用接口。它有如下的构造形式： 496 | 497 | ```c++ 498 | MovAvgFilter() 499 | ``` 500 | 501 | `MovAvgFilter` 具有低通的滤波特性，模板参数指定了滑动窗口长度。 502 | 503 | **功能** 504 | 505 | | 构造方法 | | 506 | | ------------ | ------------------ | 507 | | constructor | 默认构造 | 508 | | **成员方法** | | 509 | | coff_a | 返回反馈滤波器系数 | 510 | | coff_b | 返回前馈滤波器系数 | 511 | | reset | 重置滤波器状态 | 512 | | operator() | 滤波操作 | 513 | 514 | **示例** 515 | 516 | 使用5阶滑动均值滤波器。 517 | 518 | 构造滑动均值滤波器，并设定滑动窗口长度： 519 | 520 | ```c++ 521 | MovAvgFilter<5> flt5; 522 | std::vector datas = {0, 0.2, 0.6, 1.2, 2, 3, 4, 5}; 523 | for (auto i : datas) 524 | { 525 | std::cout << flt5(i) << std::endl; 526 | } 527 | ``` 528 | 529 | 530 | 531 | ### Savitzky-Golay型滤波器设计 532 | 533 | Savitzky-Golay型滤波器是一种在长度$k-m,...,k+m$的数据窗口中对局部数据进行$n$阶多项式拟合的数据平滑方法[^8]。 534 | 535 | ## 贝叶斯滤波器 536 | 537 | ## 参考目录 538 | 539 | [^1]: Hamilton J. D. (2020). *Time series analysis*. Princeton university press. 540 | [^2]: Oppenheim, Alan (2010). *Discrete Time Signal Processing Third Edition*. Upper Saddle River, NJ: Pearson Higher Education, Inc. 541 | [^3]: [Signal Processing (scipy.signal) — SciPy v1.10.1 Manual](https://docs.scipy.org/doc/scipy/tutorial/signal.html) 542 | [^4]:[fourier transform - How to perform spectral inversion in the frequency domain to convert a low-pass filter into a high-pass filter? - Signal Processing Stack Exchange](https://dsp.stackexchange.com/questions/61071/how-to-perform-spectral-inversion-in-the-frequency-domain-to-convert-a-low-pass) 543 | [^5]:[Mixed-Signal and DSP Design Techniques, Digital Filters (analog.com)](https://www.analog.com/media/en/training-seminars/design-handbooks/MixedSignal_Sect6.pdf) 544 | [^6]:[Bilinear transform - Wikipedia](https://en.wikipedia.org/wiki/Bilinear_transform) 545 | [^7]:Shouran M. & Elgamli E. (2020). Design and implementation of Butterworth filter. *International Journal of Innovative Research in Science, Engineering and Technology*, *9*(9). 546 | [^8]:Schmid, M., Rath, D., & Diebold, U. (2022). Why and How Savitzky–Golay Filters Should Be Replaced. *ACS Measurement Science Au*, *2*(2), 185-196. 547 | -------------------------------------------------------------------------------- /docs/Chapter 4. General program design of filter.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/Chapter 4. General program design of filter.pdf -------------------------------------------------------------------------------- /docs/chapter1.tex: -------------------------------------------------------------------------------- 1 | \chapter[short]{线性方程组} 2 | 3 | \section{矩阵正交化} 4 | \subsection{正交化原理} 5 | 对于矩阵 $A=\{v_1,v_2,\dots,v_n\}$ 找出构成该矩阵列空间极大无关组的过程叫正交化。常用的正交化手段有格拉姆-施密特（Gram-Schmidt） 6 | 正交化方法：它在选定主方向向量以后，通过不断减去其余方向在该方向上的投影得到正交基。 7 | 给出正交非归一化的传统格拉姆施密特方法： 8 | \begin{center} 9 | \begin{minipage}{.7\linewidth} 10 | \begin{algorithm}[H] 11 | \caption{\text{Non-normalized Gram-Schmidt}} 12 | \KwIn{$A=\{v_1,v_2,\dots,v_n\}$} 13 | \KwOut{$Q=\{q_1,q_2,\dots,q_n\}$} 14 | \For{$j=1\colon n$} 15 | { 16 | $v_j=x_j$\; 17 | \For{$k=1\colon j-1$} 18 | { 19 | $v_j\leftarrow v_j-(\frac{v_k^Tx_j}{v_k^Tv_k})v_k$\; 20 | } 21 | } 22 | \end{algorithm} 23 | \end{minipage} 24 | \end{center} 25 | 注意以上算法给出的是一组\emph{正交非单位}向量组，如果需要获得一组\emph{正交单位}向量组，使用以下的经典施密特算法（Classical Gram-Schmidt）： 26 | \begin{center} 27 | \begin{minipage}{.7\linewidth} 28 | \begin{algorithm}[H] 29 | \caption{\text{Classical Gram-Schmidt, CGS}} 30 | \KwIn{$A=\{v_1,v_2,\dots,v_n\}$} 31 | \KwOut{$Q=\{q_1,q_2,\dots,q_n\}$} 32 | \For{$j=1\colon n$} 33 | { 34 | $v_j=x_j$\; 35 | \For{$k=1\colon j-1$} 36 | { 37 | $v_j\leftarrow v_j-(q_k^Tx_j)v_k$\; 38 | } 39 | $q_j=v_j/\Vert v_j\Vert_2$\; 40 | } 41 | \end{algorithm} 42 | \end{minipage} 43 | \end{center} 44 | 45 | 经典的施密特正交化方法缺乏固有的数值稳定性，举如下简单的例子：在计算完成第一列正交基后，假设第二列正交基出现误差，使得$q_1^Tq_2=\delta$， 46 | 那么，在后续的 CGS 过程中，该误差并不会减小： 47 | \begin{align*} 48 | v_3 & =x_3-(q_1^Tx_3)q_1-(q_2^Tx_3)q_2 \\ 49 | q_2^Tv_3 & =q_2^Tx_3-(q_1^Tx_3)\delta -(q_2^Tx_3)=-(q_1^Tx_3)\delta \\ 50 | q_1^Tv_3 & =q_1^Tx_3-(q_1^Tx_3)-(q_2^Tx_3)\delta=-(q_2^Tx_3)\delta 51 | \end{align*} 52 | 由于数值计算是有限精度运算，正交化某列时的误差无可避免的。这使 CGS 过程中误差随着正交列增多而积累，直至错误。更多关于格拉姆 53 | 施密特正交化的误差特性，可以参考 Bjorck 的分析 \cite{BJORCK1994297}。为解决该问题，目前更普遍使用修正格拉姆施密特正交化 54 | （Modified Gram-Schmidt）方法： 55 | \begin{center} 56 | \begin{minipage}{.7\linewidth} 57 | \begin{algorithm}[H] 58 | \caption{\text{Modified Gram-Schmidt, MGS}} 59 | \KwIn{$A=\{v_1,v_2,\dots,v_n\}$} 60 | \KwOut{$Q=\{q_1,q_2,\dots,q_n\}$} 61 | \For{$j=1\colon n$} 62 | { 63 | $q_j=v_j/\Vert v_j\Vert_2$\; 64 | \For{$k=j+1\colon n$} 65 | { 66 | $v_k\leftarrow v_k-(q_k^Tv_k)q_j$\; 67 | } 68 | } 69 | \end{algorithm} 70 | \end{minipage} 71 | \end{center} 72 | 73 | 修正格拉姆施密特方法的区别在于关键的消去投影向量步骤： 74 | \begin{equation} 75 | \text{CGS}\colon v_j\leftarrow v_j-(v_k^Tx_j)v_k\qquad \text{MGS}\colon v_k\leftarrow v_k-(v_j^Tv_k)v_j 76 | \end{equation} 77 | 从 MGS 出发，考察 CGS 中遇到的问题。依然假设有不正交的向量 $q_1^Tq_2=\delta$，则 $v_3$ 的计算过程如下： 78 | \begin{align*} 79 | j & =0 \quad v_3=x_3 \\ 80 | j & =1 \quad v_3^{(1)}=v_3^{(0)}-(q_1^Tv_3^{(0)})q_1 \\ 81 | j & =2 \quad v_3^{(2)}=v_3^{(1)}-(q_2^Tv_3^{(1)})q_2 82 | \end{align*} 83 | 假设 $v_3^{(2)}$ 即为最终结果，观察误差的传播情况，首先考察对 $q_2$ 的正交性： 84 | \begin{equation} 85 | q_2^Tv_3=q_2^Tv_3^{(1)}-q_2^Tv_3^{(1)}=0 86 | \end{equation} 87 | 考察对$q_1$的正交性，由正交过程有： 88 | \begin{equation} 89 | \begin{aligned} 90 | q_1^Tv_3 & =q_1^Tv_3^{(1)}-(q_2^Tv_3^{(1)})\delta \\ 91 | q_2^Tv_3^{(1)} & =q_2^Tv_3^{(0)}-(q_1^Tv_3^{(0)})\delta \\ 92 | q_1^Tv_3^{(1)} & =q_1^Tv_3^{(0)}-q_1^Tv_3^{(0)}=0 93 | \end{aligned} 94 | \end{equation} 95 | 可发现其误差项的累计速度是乘方级的，相比于 CGS 的线性要小的多： 96 | \begin{equation} 97 | q_1^Tv_3=-(q_2^Tv_3^{(0)}-(q_1^Tv_3^{(0)})\delta)\delta=-q_2^Tv_3^{(0)}\delta+q_1^Tv_3^{(0)}\delta^2 98 | \end{equation} 99 | Bjorck\cite{BJORCK1994297} 也给出了 MGS 方法的数值特性分析。 100 | 101 | \subsection{程序实现} 102 | 按上节给出的 MGS 算法进行实现，ppx 中的函数原型如下： 103 | \begin{tcolorbox} 104 | \begin{center} 105 | \begin{minipage}{.92\linewidth} 106 | \begin{lstlisting}[language=C++] 107 | auto MGS(A:mat)->mat; 108 | \end{lstlisting} 109 | \end{minipage} 110 | \end{center} 111 | \end{tcolorbox} 112 | 113 | 下面给出具体的使用例，如对随机病态矩阵进行正交化，并给出正交化残差。通过希尔伯特矩阵构造病态矩阵 $A=H+0.00001I$ 114 | \begin{tcolorbox} 115 | \begin{center} 116 | \begin{minipage}{.92\linewidth} 117 | \begin{lstlisting}[language=C++] 118 | auto A = eye<100>(); 119 | A += hlib(A); 120 | auto Q = MGS(A); 121 | std::cout << norm1<100, 1>(Q.T() * Q - eye<100>()) << std::endl; 122 | \end{lstlisting} 123 | \end{minipage} 124 | \end{center} 125 | \end{tcolorbox} 126 | 得到残差为 2.1554e-11，而使用 CGS 方法得到残差为 2.9912。 127 | 128 | \section{QR 分解} 129 | \subsection{分解原理} 130 | 131 | 在格拉姆-施密特正交化过程中，每次由公式 $q_j=v_1-\sum_k^{j-1}(q_j^Tv_k)v_k$ 得到列向量，写出该过程变换表达式，发现变换矩阵形成 132 | 上三角格式，系数与正交投影系数相关： 133 | \begin{equation} 134 | \begin{cases} 135 | q_1 & =c_{11}v_1 \\ 136 | q_2 & =c_{12}v_1+c_{22}v_2 \\ 137 | q_3 & =c_{13}v_1+c_{32}v_2+c_{33}v_3 \\ 138 | & \vdots \\ 139 | q_n & =c_{1n}v_1+c_{2n}v_2+c_{3n}v_3+\dots c_{nn}v_n 140 | \end{cases} 141 | \end{equation} 142 | 整理成矩阵表达式： 143 | \begin{equation} 144 | \label{CGS_QR} 145 | \underbrace{\left(q_1,q_2,\dots,q_n\right)}_Q=\underbrace{\left(v_1,v_2,\dots,v_n\right)}_A 146 | \underbrace{ 147 | \begin{bmatrix} 148 | c_{11} & c_{12} & \cdots & c_{1n} \\ 149 | & c_{22} & \cdots & c_{2n} \\ 150 | & & \ddots & \vdots \\ 151 | & 152 | & & c_{nn} 153 | \end{bmatrix} 154 | }_C 155 | \end{equation} 156 | 得到 $Q=AC$。定义矩阵 $R=C^{-1}$，则由 \eqref{CGS_QR} 有矩阵分解 $A=QR$，其中 Q 是正交矩阵，而 R 是上三角矩阵。这种矩阵分解 157 | 方式称 QR 分解。使用分解矩阵代替原矩阵的好处在于：正交矩阵 Q 的特性使得参与运算的对象精度能良好保持，不会随着数值运算过程一直累计，、 158 | 而上三角矩阵作为线性代数最基础的研究对象，可以被多种技巧处理。QR 分解除了通过上节中 MGS 实现，还能通过如豪斯霍尔德（Householder） 159 | 变换，吉文斯（Givens）变换等多种方法实现。其分解本身是解超定方程组、矩阵极大无关组，奇异值分解的基础。总的来说，QR 分解是矩阵 160 | 正交分解中最重要内容之一。 161 | 162 | 对于矩阵 $A$，QR 分解目标是将其转化为上三角型与正交矩阵的乘积： 163 | \begin{equation} 164 | \begin{bmatrix} 165 | \quad & \quad & \quad \\ 166 | \quad & \quad & \quad \\ 167 | \quad & A & \quad \\ 168 | \quad & \quad & \quad \\ 169 | \quad & \quad & \quad 170 | \end{bmatrix}_{m\times n}= 171 | \begin{bmatrix} 172 | \quad & \quad & \quad \\ 173 | \quad & \quad & \quad \\ 174 | \quad & Q & \quad \\ 175 | \quad & \quad & \quad \\ 176 | \quad & \quad & \quad 177 | \end{bmatrix}_{m\times n} 178 | \begin{bmatrix} 179 | r_{11} & r_{12} & \cdots & r_{1n} \\ 180 | & r_{22} & \cdots & r_{2n} \\ 181 | & & \ddots & \vdots \\ 182 | & 183 | & & r_{nn} 184 | \end{bmatrix}_{n\times n} 185 | \end{equation} 186 | 回想 MGS 中形成矩阵 R 过程，它等价于对原矩阵 A 每列叠加初等变换矩阵：这些初等变换矩阵首先是正交矩阵，并且作用于另一矩阵后能操作其行列， 187 | 这种作用还能相互叠加。这引出了 QR 分解的另一种思路：如果存在某正交矩阵能消去矩阵的特定行列，那么累计这种矩阵作用，最终就能得到 QR 分解 188 | 中的上三角矩阵 R，而这种累计作用合成来的矩阵正是 Q 矩阵。豪斯霍尔德变换与吉文斯变换正是这种正交变换，它们能消去矩阵某列指定位置以下的元 189 | 素，或交换矩阵指定位置的元素。 190 | 191 | \subsection{Householder 变换} 192 | 豪斯霍尔德（Householder）变换，又称镜射变换，由如下公式定义： 193 | \begin{equation} 194 | H_ux=(I-2uu^{T})x 195 | \end{equation} 196 | 其中的 $H_u$ 是由 $u$（householder 因子）决定的镜射变换矩阵。如图 \ref{householder_transform} 如把矢量 $x$ 看作光线， 197 | 那么 $u$ 就是镜面方向，$H_ux$ 就是反射方向。选择待变换矩阵第 $i$ 列向量作为原始向量，选择 $e_i$ 作为反射的目标向量，记录 198 | 豪斯霍尔德变换因子 $u_i$。重复该过程于每一列中，最后记录变换矩阵的乘积为 $Q^T$，残余矩阵为上三角矩阵 $R$。 199 | \begin{figure}[htbp] 200 | \centering\label{householder_transform} 201 | \includegraphics[width=\textwidth]{HouseHolder.png} 202 | \caption{镜射变换示意图} 203 | \end{figure} 204 | 205 | 利用镜射变换方法，可以将非奇异矩阵 $A={v_1,v_2,\dots,v_n}$ 逐步转化为上三角矩阵，具体步骤如下： 206 | \begin{itemize} 207 | \item 令 $x=a_1$，求出可以使 $H_u x=\Vert a_1\Vert_2 e_1$ 的变换因子 $u_1$，且令 $a_{11}^{(1)}=\Vert a_1\Vert_2$，则有： 208 | \begin{equation*} 209 | H_{1}A=\begin{bmatrix} 210 | a_{11}^{(1)} & \mathbf{a}^{(1)} \\ 211 | 0 & A_1 212 | \end{bmatrix} 213 | \end{equation*} 214 | \item 对低一阶矩阵 $A_1$ 执行变换，有： 215 | \begin{equation*} 216 | H_{2}A_1=\begin{bmatrix} 217 | a_{22}^{(2)} & \mathbf{a}^{(2)} \\ 218 | 0 & A_2 219 | \end{bmatrix} 220 | \end{equation*} 221 | \item 重复以上过程，直到 $n-1$ 步，得到： 222 | \begin{equation} 223 | H_{n-1}A_{n-2}=\begin{bmatrix} 224 | a_{(n-1)(n-1)}^{(n-1)} & a_{(n-1)n}^{(n-1)} \\ 225 | 0 & a_{nn}^{(n-1)} 226 | \end{bmatrix} 227 | \end{equation} 228 | 令： 229 | \begin{equation*} 230 | Q=\begin{bmatrix} 231 | \mathrm{I_{n-2}} & 0 \\ 232 | 0 & \mathrm{H_{n-1}} 233 | \end{bmatrix}\cdots 234 | \begin{bmatrix} 235 | \mathrm{I_{2}} & 0 \\ 236 | 0 & \mathrm{H_{1}} 237 | \end{bmatrix} 238 | \begin{bmatrix} 239 | 1 & 0 \\ 240 | 0 & \mathrm{H_{2}} 241 | \end{bmatrix}\mathrm{H_{1}} 242 | \end{equation*} 243 | 它是正交矩阵，且有： 244 | \begin{equation*} 245 | QA=\begin{bmatrix} 246 | a_{11}^{(1)} & * & \cdots & * \\ 247 | & a_{22}^{(2)} & \cdots & * \\ 248 | & & \ddots & \vdots \\ 249 | & 250 | & & a_{nn}^{(n)} 251 | \end{bmatrix} 252 | \end{equation*} 253 | 这样，就得到了矩阵的 QR 分解 $A=Q^TR$。 254 | \end{itemize} 255 | 256 | 下面给出基于 MGS 与 Householder 变换完成 QR 分解的算法： 257 | \begin{center} 258 | \begin{minipage}{.7\linewidth} 259 | \begin{algorithm}[H] 260 | \caption{\text{QR factorization with MGS}} 261 | \KwIn{$A=\{x_1,x_2,\dots,x_n\}$} 262 | \KwOut{$Q=\{q_1,q_2,\dots,q_n\}$} 263 | \For{$j=1\colon n$} 264 | { 265 | $v_j=x_j$\; 266 | \For{$k=j+1\colon n$} 267 | { 268 | $r_{jj}=\Vert v_j\Vert_2$\; 269 | $q_j=v_j/r_{jj}$\; 270 | \For{$k=j+1:n$} 271 | { 272 | $r_{jk}=q_j^T v_k$\; 273 | $v_k\leftarrow v_k - r_{jk}q_j$\; 274 | } 275 | } 276 | } 277 | \end{algorithm} 278 | \end{minipage} 279 | \end{center} 280 | 给出基于 Householder 变换的的 QR 分解算法： 281 | \begin{center} 282 | \begin{minipage}{.7\linewidth} 283 | \begin{algorithm}[H] 284 | \caption{\text{QR factorization with Householder}} 285 | \KwIn{$A=\{x_1,x_2,\dots,x_n\}$} 286 | \KwOut{$Q=\{q_1,q_2,\dots,q_n\}$} 287 | $m, n \leftarrow \operatorname{shape}(A)$\; 288 | $R \leftarrow \operatorname{copy}(A)$\; 289 | $Q \leftarrow I_{m}$\; 290 | \For{$k=0\colon n-1$}{ 291 | $\mathbf{u} \leftarrow \operatorname{copy}\left(R_{k:, k}\right)$\; 292 | $u_{0} \leftarrow u_{0}+\operatorname{sgn}\left(u_{0}\right)\vert\mathbf{u}\vert$\; 293 | $\mathbf{u} \leftarrow \mathbf{u} /\Vert\mathbf{u}\Vert_2$\; 294 | $R_{k:, k:} \leftarrow R_{k:, k:}-2 \mathbf{u}\left(\mathbf{u}^{\top} R_{k:, k:}\right)$\; 295 | $Q_{k:,:} \leftarrow Q_{k:,:}-2 \mathbf{u}\left(\mathbf{u}^{\top} Q_{k:,:}\right)$\; 296 | } 297 | \end{algorithm} 298 | \end{minipage} 299 | \end{center} 300 | 这种 QR 分解的实现方式简单易懂，但会生成大量中间变量。事实 Lapack 中的QR分解将返回： 301 | \begin{equation*} 302 | \left[\begin{array}{lllll} u_{11} & r_{11} & r_{21} & ... \\ 303 | u_{12} & u_{21} & r_{22} & ... \\ 304 | u_{13} & u_{22} & u_{31} & ... \\ 305 | u_{14} & u_{23} & u_{32} & ... \\ 306 | ... & ... & ... & ...\end{array}\right] 307 | \end{equation*} 308 | 即上三角矩阵 R 存储于返回矩阵的上三角部分，镜射变换因子 $u_i$ 储存于下三角中，而 R 的对角线单独返回。对于需要 Q 的情形， 309 | 使用变换因子生成： 310 | \begin{equation*} 311 | Q_i=I-c_iu_i u_i^{T} 312 | \end{equation*} 313 | 如不显式需要 Q，则分解中每次镜射变换因子生成变换矩阵作用在 A 上过程变为： 314 | \begin{equation*} 315 | Q_{i}x=(I-c_iu_{i}u_{i}^{T})x=x-c_iu_{i}u_{i}^Tx=x-c_iu_{i}(u_{i}^Tx) 316 | \end{equation*} 317 | 这种方法也称为隐式 QR 分解。 318 | 319 | \subsection{使用实例} 320 | 以上讨论的 QR 分解方法是在矩阵 A 可逆情况下得出的。但同样适用于列满秩情形，以矩阵： 321 | \begin{equation*} 322 | A=\begin{bmatrix} 323 | 1 & -4 \\ 324 | 2 & 3 \\ 325 | 2 & 2 326 | \end{bmatrix} 327 | \end{equation*} 328 | 为例，它是一个 $3\times 2$ 的列满秩矩阵，它将被分解为 Q（3 阶正交矩阵）与 R（2阶上三角矩阵），代码如下： 329 | \begin{tcolorbox} 330 | \begin{center} 331 | \begin{minipage}{.92\linewidth} 332 | \begin{lstlisting}[language=C++] 333 | MatrixS<3, 2> A{{1, 2, 2}, {-4, 3, 2}}; 334 | QR<3, 2> qrdcmp(A); 335 | std::cout << qrdcmp.A<< "\n"; 336 | std::cout << qrdcmp.d << "\n"; 337 | std::cout << qrdcmp.c << "\n"; 338 | \end{lstlisting} 339 | \end{minipage} 340 | \end{center} 341 | \end{tcolorbox} 342 | 其中 A 以紧凑形式返回了 QR 分解结果，d 代表对角线元素，c 代表 Householder 变换因子系数倒数，运行结果如下： 343 | \begin{tcolorbox} 344 | \begin{center} 345 | \begin{minipage}{.92\linewidth} 346 | \begin{lstlisting}[language=C++] 347 | MAT<3,2>: 348 | 4 -2 349 | 2 9 350 | 2 3 351 | ARR<2>: -3 -5 352 | ARR<2>: 12 45 353 | \end{lstlisting} 354 | \end{minipage} 355 | \end{center} 356 | \end{tcolorbox} 357 | 现在从其中还原 QR 分解矩阵：紧凑形式 A 中的上三角部分与对角线矩阵 d 组合得到 R，下三角部分每一列是变换向量，配合变换因子 358 | 可以还原出每次作用的 Householder 矩阵，累乘这些矩阵得到 $Q^T$： 359 | \begin{equation*} 360 | R=\begin{bmatrix} 361 | -3 & -2 \\ 362 | 0 & -5 \\ 363 | 0 & 0 364 | \end{bmatrix} 365 | \end{equation*} 366 | 变换因子为：$u_1=(4,2,2)^T,u_2=(9,3)^T$，其对应系数为 $c=(\frac{1}{12},\frac{1}{45})^T$，由公式 $H_u=I-cuu^T$ 求出其 Householder 矩阵为： 367 | \begin{equation*} 368 | H_1=\frac{1}{3} \begin{bmatrix} 369 | -1 & -2 & -2 \\ 370 | -2 & 2 & -1 \\ 371 | -2 & -1 & 2 372 | \end{bmatrix}, 373 | H_2=\frac{1}{5} \begin{bmatrix} 374 | -4 & -3 \\ 375 | -3 & 4 376 | \end{bmatrix} 377 | \end{equation*} 378 | 验证 Householder 矩阵正确性，将其左作用于矩阵 A： 379 | \begin{equation*} 380 | H_1A=\begin{bmatrix} 381 | -3 & -2 \\ 382 | 0 & 4 \\ 383 | 0 & 3 384 | \end{bmatrix},H_2^{'}H_1A=\begin{bmatrix} 385 | 1 & 0 \\ 386 | 0 & H_2 387 | \end{bmatrix}H_1A=\begin{bmatrix} 388 | -3 & -2 \\ 389 | 0 & -5 \\ 390 | 0 & 0 391 | \end{bmatrix} 392 | \end{equation*} 393 | 得到正确的上三角形式 R，故由 $Q^T=H_2H_1$可知： 394 | \begin{equation*} 395 | Q=(Q^T)^T=(H_2H_1)^T=\begin{bmatrix} 396 | -\frac{1}{3} & \frac{14}{15} & \frac{2}{15} \\ 397 | -\frac{2}{3} & -\frac{1}{3} & -\frac{2}{3} \\ 398 | -\frac{2}{3} & -\frac{1}{3} & \frac{11}{15} 399 | \end{bmatrix} 400 | \end{equation*} 401 | 与 Matlab 计算结果相比，QR 分解相差了 $\pm$ 符号，这是由于正交化方向选择不同引起的，本质并无区别。在之后的 SVD 分解与特征值计算中，依然会出现该现象。 -------------------------------------------------------------------------------- /docs/chapter4.tex: -------------------------------------------------------------------------------- 1 | \chapter{一般滤波器的程序设计} 2 | 3 | 对信号的时间演化过程进行估计十分常见。一般来说，信号可以分为周期信号（有限带宽信号、确定性信号）与随机信号（无限带宽信号、 4 | 随机信号）。对于确定信号滤波侧重于对信号不同频谱分量的取舍，例如低通滤波器保留信号的低频分量，丢弃信号的高频分量。因此这种 5 | 滤波器也称作频率成型滤波器\cite{oppenheim1997signals}。对于随机信号，滤波侧重其功率谱不同分量的取舍，在时间域对应的操作 6 | 就是对自相关函数的修改。随机信号滤波本质是对其时间统计特征和概率特征的成型。因此，滤波器的设计准则也将基于相关统计特征， 7 | 这和确定性信号滤波有显著的不同。但这两种滤波方式并非对立，对随机过程的滤波操作都可以归纳到统一数学框架中，视作在频率与 8 | 时域上修改了统计特性。根据使用滤波器目的不同与信号本身特性，不同滤波方法只是各自侧重比例不同而已。 9 | 10 | 假设要对某随机过程 $t=n$ 时刻的值进行估计，根据可用数据因果性，有三种主要的任务形式\cite{theodoridis2020machine}： 11 | \begin{itemize} 12 | \item 滤波，在时刻 $n$ 的估计是基于所有先前的接受（测量）输入信息，\emph{直到并包括}当前时刻 $n$。 13 | \item 平滑，首先收集时间区间 $[0,N]$ 内的数据，使用区间 $[0,N]$ 中\emph{所有}可用信息，得到每个时刻 $t\le N$ 的估计。 14 | \item 预测，基于直到包括时刻 $n$ 的信息，得到 $n+ \tau,\tau\ge 0$ 的估计。 15 | \end{itemize} 16 | 17 | 举一个时变随机过程例子：时刻 $n$ 的输出变量是 $y_n$，其值取决于相应的输入向量 $x_n$ 包含的观测值。在滤波中， 18 | 后者只能包括时刻 $n,n-1,\dots ,0$。索引集中这种限制表明了因果关系。相反，在平滑过程中，除了过去时刻的数据，还能使用 19 | 未来时刻的信息，即 $\dots ,n+2,n+1,n,n-1,\dots $。 20 | 21 | 三种任务形式本质上同源，只是根据时间因果性进行区分。因此很多算法稍作改变就能互通：如 Savizkg-Golay 平滑算法被称作 22 | Savizkg-Golay 滤波器；均值滤波器的与滑动平均有同样的数学表达。在本章除有特殊情况，以滤波算法代替作为说明。 23 | 24 | \section{线性滤波} 25 | 26 | 线性滤波指滤波器输出是输入\emph{线性叠加}的滤波器。由于它的频域特性是固定的，因此最适用于处理周期信号，也因为结构简单， 27 | 除一般软件实现外，还广泛存在于硬件电子电路与机械物理结构中。 28 | 29 | \subsection{基本特性} 30 | 数字滤波器的数学模型如下：设输入信号为 $x(n)$，输出信号为 $y(n)$，滤波过程可用以下差分方程来表示\cite{scipy_docs_filtering}: 31 | \begin{equation} 32 | \label{flt_time_domain} 33 | a_0y_{n}=\sum_{i=0}^{N} b_{i} x_{n-i}-\sum_{j=1}^{M-1} a_{j} y_{n-j} 34 | \end{equation} 35 | 36 | 其对应的传递函数\footnote[1]{请注意识别系数中的项数与 $a,b$ 代表的含义，它们往往在不同文档与程序中不同甚至相反。}为： 37 | \begin{equation} 38 | H(z)=\frac{\sum\limits_{i=0}^{N} b_iz^{-i}}{1+\sum\limits_{i=1}^{M-1} a_iz^{-i}} 39 | \end{equation} 40 | 41 | 它对单位冲激函数的响应为： 42 | \begin{equation} 43 | \begin{split} 44 | \label{h_resp} 45 | h_n=z^{-1}(H(z))\\ 46 | y_n=\sum_{i=-\infty}^{\infty} h_ix_{n-i} 47 | \end{split} 48 | \end{equation} 49 | 50 | 设计线性滤波器就是找到合适的 $h(n)$，使输出信号满足使用需求。通过观察式 \eqref{h_resp} 给出的时域输出，可以讨论一些数字滤波器中 51 | 共通的基本特性与常见概念。首先是递归特性（Recursive Filter）： 52 | 53 | 当 $a_0=1$，其余 $a_k$ 均为零时，称滤波器是非递归的，又称\emph{有限冲激响应}（Finite Impulse Response, FIR）滤波器；该类型 54 | 滤波器也称为移动平均（MA）滤波器。当 $a_0 = 1$，其余 $a_k$ 不全为零时，称滤波器为递归的，又称\emph{无限冲激响应}（Infinite 55 | Impulse Response, IIR）滤波器；该类型滤波器根据是否满足 $b_0 = 1$ 且 $b_k = 0$，又分为自回归（AR）滤波器与自回归 56 | 移动平均（ARMA）滤波器\cite{hamilton2020time}。值得注意的是：虽然一般将递归与 IIR 滤波器对应，非递归与 FIR 滤波器对应，但它们之 57 | 间关系不是严格等同的，递归同样适用于 FIR 滤波器实现，非递归也可实现 IIR 滤波器。 58 | \begin{tcolorbox} 59 | \textrm{根据随机过程区分类型：} 60 | \begin{align*} 61 | y_{n} & =\sum_{i=0}^{N} b_{i} x_{n}\qquad & \textrm{MA，移动平均过程} \\ 62 | y_{n} & =x_{n}-\sum_{j=1}^{M-1} a_{j} y_{n-j}\qquad & \textrm{AR，自回归过程} \\ 63 | y_{n} & =\sum_{i=0}^{N} b_{i} x_{n}-\sum_{j=1}^{M-1} a_{j} y_{n-j}\qquad & \textrm{ARMA，自回归移动平均过程} 64 | \end{align*} 65 | \end{tcolorbox} 66 | 67 | 68 | 其次是因果性（Causal Filter）：如果对于 $n< 0$ 时，脉冲响应系数 $h(n)$ 不为零，则说明系统某时刻输出与未来输入相关，不具有因果性； 69 | 否则称滤波器是\emph{因果系统}。当冲激响应系数满足以下条件时，称系统是因果且稳定的： 70 | \begin{equation} 71 | \begin{cases} 72 | h(n)=0, n\le 0 \\ 73 | \sum_{n=0}^{\infty} \vert h(n) \vert \le C 74 | \end{cases} 75 | \end{equation} 76 | 77 | 这些概念都容易从直观上解释：例如对于某滤波器，等式右边项 $y$ 相关系数全零时，系统只与过去时刻输入有关，因此具有有限项、因果性、 78 | 绝对稳定性；当右边项 $y$ 相关系数不为全为零，相应系数的引入相当于对原系统增加反馈环节，等同于追加入无穷多项输入，改变了原有 79 | 因果与稳定性。 80 | 81 | \subsection{FIR 滤波器} 82 | 83 | 在线性滤波器中，FIR 滤波器又是最简单的滤波器：它等价于开环系统，拥有绝对稳定性，并且容易设计出\emph{线性相位}\cite{enwiki:1140890375}系统。 84 | 这使得设计良好的 FIR 滤波器对周期信号的行为是十分明确易知的。 85 | \subsubsection{相幅特性} 86 | 式 \eqref{h_resp} 给出了滤波器的频幅响应与相位偏移。考虑到任务因果性，滤波器对于负时间的响应为零。对于输入长度为 $N$，采样周期 87 | 为 $T$ 的信号序列，滤波器输出响应与信号序列中点 $M$ 有如下的关系： 88 | \begin{itemize} 89 | \item \uppercase\expandafter{\romannumeral1} 型\footnote[1]{这种分类在使用滤波器程序设计工具，如 fir1 时常见。}：信号序列长度是奇数 90 | $N=2M+1$，存在确实中点 $M$，且系数 $h[n]$ 围绕该中点偶对称分布： 91 | \begin{align*} 92 | \mathrm{H}(j \Omega) & =\sum_{n=-M}^{M} h[n] \cdot e^{-j \Omega T(n+M)} \\ 93 | & =e^{-j \Omega T M} \sum_{n=-M}^{M} h[n] \cdot e^{-j \Omega Tn} \\ 94 | & =e^{-j \Omega T M} 95 | \left\{ 96 | h[M]+\sum_{n=0}^{M-1} h[n] \cdot\left(e^{j \Omega T n}+e^{-j \Omega T n}\right) 97 | \right\} \\ 98 | & =e^{-j \Omega T M}\left\{h[M]+\sum_{n=0}^{M-1} h[n] \cdot 2 \cos (\Omega T n)\right\} 99 | \end{align*} 100 | 表明该系统对频率 $\Omega$ 谐波会产生 $-\Omega TM$ 线性相位偏移，此斜率即\emph{群延迟}\cite{oppenheim1997signals}。 101 | FIR 滤波器有着线性相位偏移：即对任何频率谐波成分，有同样的群延迟，对应于整个系统输入延迟 $TM$ 时间。注意线性相位是由 $h(n)=h(2M-n)$ 102 | 保证的。 103 | \item \uppercase\expandafter{\romannumeral2} 型：任务信号序列长度是偶数 $N=2M$，存在虚拟中点 $M-\frac{1}{2}$，且系数 $h[n]$ 围绕该 104 | 中点偶对称分布： 105 | \begin{align*} 106 | \mathrm{H}(j \Omega) & =\sum_{n=-M}^{M-1} h[n] \cdot e^{-j \Omega T(n+M)} \\ 107 | & =\sum_{n=\frac{1}{2}}^{M-\frac{1}{2}} 108 | \left\{ 109 | h[M-\frac{1}{2}-n] \cdot e^{-j \Omega T(M-\frac{1}{2}-n)}+ 110 | h[M-\frac{1}{2}+n] \cdot e^{-j \Omega T(M-\frac{1}{2}+n)} 111 | \right\} \\ 112 | & =\sum_{n=0}^{M-1} 113 | \left\{ 114 | h[M-n-1] \cdot e^{-j \Omega T(M-n-1)}+ h[M+n] \cdot e^{-j \Omega T(M+n)} 115 | \right\} \\ 116 | & =e^{-j \Omega T (M-\frac{1}{2})} 117 | \left\{ 118 | \sum_{n=0}^{M-1} h[M+n] \cdot\left(e^{j \Omega T(n+\frac{1}{2})}+e^{-j \Omega T(n+\frac{1}{2})}\right) 119 | \right\} \\ 120 | & =e^{-j \Omega T (M-\frac{1}{2})} 121 | \left\{ 122 | \sum_{n=0}^{M-1} h[M+n] \cdot 2 \cos \left[\Omega T (n+\frac{1}{2})\right] 123 | \right\} 124 | \end{align*} 125 | 表明该系统对频率 $\Omega$ 谐波会产生 $-\Omega T(M-\frac{1}{2})$ 线性相位偏移，当 $\Omega=\pi$ 时， 126 | $\cos \left[\Omega T (n+\frac{1}{2})\right]=0$，故 $\mathrm{H}(j\Omega)=0$，即该类型滤波器 127 | 不能用于$H(\pi)\ne 0$ 的滤波任务，此时线性相位特性与实现高通、带阻滤波器要求相矛盾。 128 | \item \uppercase\expandafter{\romannumeral3} 型：任务信号序列长度是奇数 $N=2M+1$，存在确实中点 $M$，且系数 $h[n]$ 围绕该中点奇对称分布： 129 | \begin{align*} 130 | \mathrm{H}(j \Omega) & =\sum_{n=-M}^{M} h[n] \cdot e^{-j \Omega T(n+M)} \\ 131 | & =e^{-j \Omega T M} \sum_{n=-M}^{M} h[n] \cdot e^{-j \Omega Tn} \\ 132 | & =e^{-j \Omega T M} 133 | \left\{ 134 | \sum_{n=0}^{M-1} h[n] \cdot\left(e^{j \Omega T n}-e^{-j \Omega T n}\right) 135 | \right\} \\ 136 | & =-je^{-j \Omega T M}\left\{\sum_{n=0}^{M-1} h[n] \cdot 2 \sin (\Omega T n)\right\} 137 | \end{align*} 138 | 表明该系统对频率 $\Omega$ 谐波会产生 $-(\Omega+\frac{\pi}{2}) TM$ 线性相位偏移，由于在 $\Omega=\pi$ 处频幅响应为零， 139 | 不能用于 $H(\pi)\ne 0, H(0)\ne 0$ 的滤波任务，可以实现带通滤波器。 140 | \item \uppercase\expandafter{\romannumeral4} 型：任务信号序列长度是偶数 $N=2M$，存在虚拟中点 $M-\frac{1}{2}$，且系数 $h[n]$ 围绕 141 | 中点奇对称分布： 142 | \begin{align*} 143 | \mathrm{H}(j \Omega) & =\sum_{n=-M}^{M-1} h[n] \cdot e^{-j \Omega T(n+M)} \\ 144 | & =\sum_{n=\frac{1}{2}}^{M-\frac{1}{2}} 145 | \left\{ 146 | h[M-\frac{1}{2}-n] \cdot e^{-j \Omega T(M-\frac{1}{2}-n)}+ 147 | h[M-\frac{1}{2}+n] \cdot e^{-j \Omega T(M-\frac{1}{2}+n)} 148 | \right\} \\ 149 | & =\sum_{n=0}^{M-1} 150 | \left\{ 151 | h[M-n-1] \cdot e^{-j \Omega T(M-n-1)}+ h[M+n] \cdot e^{-j \Omega T(M+n)} 152 | \right\} \\ 153 | & =e^{-j \Omega T (M-\frac{1}{2})} 154 | \left\{ 155 | \sum_{n=0}^{M-1} h[M+n] \cdot\left(e^{j \Omega T(n+\frac{1}{2})}-e^{-j \Omega T(n+\frac{1}{2})}\right) 156 | \right\} \\ 157 | & =-je^{-j \Omega T (M-\frac{1}{2})} 158 | \left\{ 159 | \sum_{n=0}^{M-1} h[M+n] \cdot 2 \sin \left[\Omega T (n+\frac{1}{2})\right] 160 | \right\} 161 | \end{align*} 162 | 表明该系统对频率 $\Omega$ 谐波会产生 $-\Omega T(M-\frac{1}{2})$ 线性相位偏移，并且当 $\Omega=0,2\pi$ 163 | 时频幅响应为零，不能实现低通、带阻滤波器。注意以上所有要求与分析只适用于因果线性相位 FIR 滤波器。FIR 滤波器不需要满足这些特性时， 164 | 项数与系数、实现的滤波器类型之间没有关系。 165 | \end{itemize} 166 | 167 | \subsubsection{窗函数法} 168 | 169 | FIR 滤波器的系数设计有多种方法。最常见的一种是首先根据滤波类型（低通/高通/带通/带阻）选择理想的频率响应，然后与频域上特定形状的函数 170 | 做卷积，达到频率选择成型的目的。该过程中使用的函数称作窗函数，这种方法称为窗函数法。 171 | \begin{figure}[htbp] 172 | \centering\label{ideal_lp_g_resp} 173 | \includegraphics[width=0.5\textwidth]{ideal_lp_flt_am_G.png} 174 | \caption{理想低通滤波器幅值频幅响应} 175 | \end{figure} 176 | 177 | 对于给定的 FIR 系统，其时域输出由输入频谱 $X(\Omega)$ 和滤波器频响 $H(\Omega)$ 乘积的逆离散傅里叶变换（$\rm{DTFT^{-1}}$）给出： 178 | \begin{equation} 179 | \label{fir_y_t} 180 | y(n)=\textrm{DTFT}^{-1}_n(H\cdot X)=\frac{1}{2\pi}\int_{-\pi}^{\pi}H(\Omega)X(\Omega)e^{j\Omega n}d\Omega 181 | \end{equation} 182 | 其中 $\rm{DTFT}$ 定义为： 183 | $$ 184 | \textrm{DTFT}_{\Omega}(x)=\sum_{n\in \mathbb{Z}}x(n)e^{-j\Omega n} 185 | $$ 186 | 输出信号的频谱形状决定了式\eqref{fir_y_t}中冲激响应系数计算方式。以设计理想低通滤波器为例，其频谱应在频率从零到截至频率 $f_c$ 的通频带 187 | 内具有单位增益，在截止频率 $f_c$ 以上时，具有增益零（图\ref{ideal_lp_g_resp}）： 188 | \begin{equation*} 189 | H(\Omega)=\begin{cases} 1\quad(-\Omega_c\le\Omega\le \Omega_c)\\ 0\quad(else) \end{cases} 190 | \end{equation*} 191 | 为找到满足该特性的响应系数，作逆离散傅里叶变换： 192 | \begin{equation} 193 | h[n]=\frac{1}{2\pi}\int_{-\Omega_c}^{\Omega_c}{1\cdot e^{j\Omega n}d\Omega} 194 | % =\frac{1}{2\pi}[\frac{e^{j\Omega n}}{jn}]_{-\Omega_c}^{\Omega_c} 195 | =\frac{\sin(2\pi f_c n)}{n \pi} 196 | =\Omega_c \frac{\sin \left(n \Omega_{c}\right)}{\left(n \Omega_{c}\right)} 197 | =B \textrm{sinc}(Bn) 198 | \end{equation} 199 | 其中 $\Omega_c=2\pi f_c$ 为角频率，$B = 2f_c$ 为通频带宽，$\textrm{sinc}$ 是\emph{辛格函数}。同理，可以得出理想高通、带通、带阻滤波 200 | 器的响应系数表达式： 201 | \begin{table}[!h] 202 | \caption{理想滤波器频幅响应系数} 203 | \centering 204 | \begin{tabular}{ccc} 205 | \hline 206 | 类型 & $h[n],n\ne0$ & $h[n],n=0$ \\ \hline 207 | 低通 & $B\mathrm{sinc}(Bn)$ & $B$ \\ 208 | 高通 & -$B\mathrm{sinc}(Bn)$ & $1-B$ \\ 209 | 带通 & $B_2\mathrm{sinc}(B_2n)-B_1\mathrm{sinc}(B_1n)$ & $B_2-B_1$ \\ 210 | 带阻 & $B_2\mathrm{sinc}(B_2n)+B_1\mathrm{sinc}(B_1n)$ & $B_2+B_1$ \\ \hline 211 | \end{tabular} 212 | \end{table} 213 | 214 | 由于理想滤波器输出由输入信号与响应系数卷积（$y[n]=x * h$）给出，但实际上只能求有限时间卷积和，因此将无法避免出现滤波器频谱变形。这种有限截断 215 | 导出的滤波器频域特性和原本理想滤波器区别主要在于频响有相当明显振铃： 216 | \begin{figure}[htbp] 217 | \centering\label{finite_trunc_ideal_filter} 218 | \includegraphics[width=\textwidth]{finite_trunc_ideal_filter.png} 219 | \caption{有限截断产生吉布斯现象} 220 | \end{figure} 221 | 222 | 为消除这些振铃，需要对频谱使用一些函数进行二次加工，这些函数称作\emph{窗函数}。窗函数能较好改变频谱特性，与理想滤波器一起构成了实际 FIR 223 | 滤波器。以海明窗（Hamming）为例：它时域上形状是钟形曲线，而频域上则对主瓣以外成分有强烈衰减。具有这种特性的窗函数作用于理想滤波器以后，会 224 | 大幅度削减截断频率处的波纹。常见窗函数表达式与设计方法可以参考\cite[\href{https://ccrma.stanford.edu/~jos}{JULIUS O. SMITH III}] 225 | {smith2007introduction}提供的内容。 226 | $$\textrm{海明窗}\quad w[n]=0.54-0.46\cos(\frac{2\pi n}{N-1})$$ 227 | \begin{figure}[htbp] 228 | \centering\label{hamming_resp} 229 | \includegraphics[width=0.8\textwidth]{hamming_resp.png} 230 | \caption{海明窗时域与频域形状} 231 | \end{figure} 232 | 233 | 将窗函数叠加到理想滤波器系数，就得到最终 FIR 滤波器系数 $h[n]\leftarrow h[n]*w[n]$。图\ref{ideal_hamming_comp} 说明了加窗对滤波器性能的 234 | 影响：首先是截至频率（$f_c=0.2$）处的振铃消失，此外滤波器通带与阻带的区别越发明显。 235 | \begin{figure}[htb] 236 | \centering\label{ideal_hamming_comp} 237 | \includegraphics[width=\textwidth]{ideal_hamming_comp.png} 238 | \caption{理想低通滤波器幅值频幅响应} 239 | \end{figure} 240 | \newpage 241 | 242 | \subsection{程序实现} 243 | 线性滤波的程序实现围绕式\ref{flt_time_domain}进行。该式表明数字线性滤波器是两项卷积之和，每项卷积有图\ref{lin_flt_impl} 表示的加乘累计 244 | 结构。这种结构易于电路与软件实现：置换卷积对应于固定位置索引与先进先出（FIFO）的队列元素的乘积和。将滤波器系数置入固定数组，然后将队列元素 245 | 依次加成就得到滤波结果。 246 | \begin{figure}[!h] 247 | \centering\label{lin_flt_impl} 248 | \includegraphics[width=\textwidth]{lin_flt_impl.png} 249 | \caption{数字滤波器的程序实现} 250 | \end{figure} 251 | 252 | \subsubsection{用户接口} 253 | \textbf{Filter} 类是 ppx 实现的一般统计线性滤波器设计与使用接口，定义对应有理传递函数有如下形式： 254 | $$ 255 | Y(z)=\frac{b(1)+b(2) z^{-1}+\ldots+b\left(n_{b}+1\right) z^{-n_b}}{1+a(2) z^{-1}+\ldots+a\left(n_{a}+1\right) z^{-n_{a}}} X(z) 256 | $$ 257 | 这种形式能同时处理FIR与IIR型滤波器，其中 $n_a$ 是反馈滤波器阶数，$n_b$ 是前馈滤波器阶数。由归一化设定 $a(1)=1$。 258 | 该类型提供接口如下： 259 | \begin{table}[!h] 260 | \centering 261 | \begin{tabular}{cc} 262 | \toprule 263 | \emph{构造方法} & \\ 264 | \midrule 265 | constructor & 滤波器类型：低通（默认值）/高通/带通/带阻 \\ 266 | \midrule 267 | \emph{成员方法} & \\ 268 | \midrule 269 | coff\_a & 返回反馈滤波器系数 \\ 270 | coff\_b & 返回前馈滤波器系数 \\ 271 | reset & 重置滤波器状态 \\ 272 | operator() & 滤波操作 \\ 273 | \bottomrule 274 | \end{tabular} 275 | \end{table} 276 | 277 | \subsubsection{使用示例} 278 | 使用有理传递函数 $H(z)=\frac{1}{1-0.9z^{-1}}$ 对数据进行滤波。创建由正弦谐波与白噪声组合的输入数据： 279 | 280 | \begin{lstlisting}[language=C++] 281 | #include 282 | 283 | int main() { 284 | std::cout << "Hello, World!" << std::endl; 285 | return 0; 286 | } 287 | \end{lstlisting} 288 | \newpage 289 | \bibliographystyle{unsrt} 290 | \bibliography{references} -------------------------------------------------------------------------------- /docs/graphics/HouseHolder.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/graphics/HouseHolder.png -------------------------------------------------------------------------------- /docs/graphics/finite_trunc_ideal_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/graphics/finite_trunc_ideal_filter.png -------------------------------------------------------------------------------- /docs/graphics/hamming_resp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/graphics/hamming_resp.png -------------------------------------------------------------------------------- /docs/graphics/ideal_hamming_comp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/graphics/ideal_hamming_comp.png -------------------------------------------------------------------------------- /docs/graphics/ideal_lp_flt_am_G.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/graphics/ideal_lp_flt_am_G.png -------------------------------------------------------------------------------- /docs/graphics/lin_flt_impl.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/graphics/lin_flt_impl.png -------------------------------------------------------------------------------- /docs/img/image-20230522223735541.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230522223735541.png -------------------------------------------------------------------------------- /docs/img/image-20230524215315175.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230524215315175.png -------------------------------------------------------------------------------- /docs/img/image-20230524220454952.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230524220454952.png -------------------------------------------------------------------------------- /docs/img/image-20230524235020703.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230524235020703.png -------------------------------------------------------------------------------- /docs/img/image-20230525225025440.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230525225025440.png -------------------------------------------------------------------------------- /docs/img/image-20230528211249065.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230528211249065.png -------------------------------------------------------------------------------- /docs/img/image-20230529234937685.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230529234937685.png -------------------------------------------------------------------------------- /docs/img/image-20230530000335488.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230530000335488.png -------------------------------------------------------------------------------- /docs/img/image-20230604113620149.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230604113620149.png -------------------------------------------------------------------------------- /docs/img/image-20230604140200828.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/docs/img/image-20230604140200828.png -------------------------------------------------------------------------------- /docs/references.bib: -------------------------------------------------------------------------------- 1 | @article{BJORCK1994297, 2 | title = {Numerics of Gram-Schmidt orthogonalization}, 3 | journal = {Linear Algebra and its Applications}, 4 | volume = {197-198}, 5 | pages = {297-316}, 6 | year = {1994}, 7 | issn = {0024-3795}, 8 | doi = {https://doi.org/10.1016/0024-3795(94)90493-6}, 9 | url = {https://www.sciencedirect.com/science/article/pii/0024379594904936}, 10 | author = {Å. Björck}, 11 | abstract = {The Gram-Schmidt (GS) orthogonalization is one of the fundamental procedures in linear algebra. In matrix terms it is equivalent to the factorization AQ1R, where Q1∈Rm×n with orthonormal columns and R upper triangular. For the numerical GS factorization of a matrix A two different versions exist, usually called classical and modified Gram-Schmidt (CGS and MGS). Although mathematically equivalent, these have very different numerical properties. This paper surveys the numerical properties of CGS and MGS. A key observation is that MGS is numerically equivalent to Householder QR factorization of the matrix A augmented by an n×n zero matrix on top. This can be used to derive bounds on the loss of orthogonality in MGS, and to develop a backward-stable algorithm based on MGS. The use of reorthogonalization and iterated CGS and MGS algorithms are discussed. Finally, block versions of GS are described.} 12 | } 13 | 14 | @inbook{theodoridis2020machine, 15 | title = {Machine Learning: A Bayesian and Optimization Perspective}, 16 | author = {Theodoridis, Sergios}, 17 | year = {2020}, 18 | publisher = {Academic Press}, 19 | chapter = 4, 20 | pages = {134-135} 21 | } 22 | @misc{scipy_docs_filtering, 23 | howpublished = {\url{https://docs.scipy.org/doc/scipy/tutorial/signal.html#difference-equation-filtering}}, 24 | note = {Accessed September 27, 2023}, 25 | title = {{{SciPy} 1.1.3: Fundamental Algorithms for Scientific 26 | Computing in Python}}, 27 | author = {Virtanen, Pauli and Gommers, Ralf and Oliphant, Travis E. and 28 | Haberland, Matt and Reddy, Tyler and Cournapeau, David and 29 | Burovski, Evgeni and Peterson, Pearu and Weckesser, Warren and 30 | Bright, Jonathan and {van der Walt}, St{\'e}fan J. and 31 | Brett, Matthew and Wilson, Joshua and Millman, K. Jarrod and 32 | Mayorov, Nikolay and Nelson, Andrew R. J. and Jones, Eric and 33 | Kern, Robert and Larson, Eric and Carey, C J and 34 | Polat, {\.I}lhan and Feng, Yu and Moore, Eric W. and 35 | {VanderPlas}, Jake and Laxalde, Denis and Perktold, Josef and 36 | Cimrman, Robert and Henriksen, Ian and Quintero, E. A. and 37 | Harris, Charles R. and Archibald, Anne M. and 38 | Ribeiro, Ant{\^o}nio H. and Pedregosa, Fabian and 39 | {van Mulbregt}, Paul and {SciPy 1.0 Contributors}} 40 | } 41 | @book{smith2007introduction, 42 | title = {Introduction to digital filters: with audio applications}, 43 | author = {Smith, Julius Orion}, 44 | volume = {2}, 45 | year = {2007}, 46 | publisher = {Julius Smith} 47 | } 48 | @book{hamilton2020time, 49 | title = {Time series analysis}, 50 | author = {Hamilton, James D}, 51 | year = {2020}, 52 | publisher = {Princeton university press} 53 | } 54 | @book{oppenheim1997signals, 55 | title = {Signals and systems}, 56 | author = {Oppenheim, Alan V and Willsky, Alan S and Nawab, Syed Hamid and Ding, Jian-Jiun}, 57 | volume = {2}, 58 | year = {1997}, 59 | publisher = {Prentice hall Upper Saddle River, NJ} 60 | } 61 | @misc{enwiki:1140890375, 62 | author = {{Wikipedia contributors}}, 63 | title = {Linear phase --- {Wikipedia}, The Free Encyclopedia}, 64 | year = {2023}, 65 | url = {https://en.wikipedia.org/w/index.php?title=Linear_phase&oldid=1140890375}, 66 | note = {[Online; accessed 17-October-2023]} 67 | } -------------------------------------------------------------------------------- /docs/template.tex: -------------------------------------------------------------------------------- 1 | \documentclass[12pt, a4paper, twoside]{ctexrep} 2 | \usepackage{amsmath, amsthm, amssymb, appendix, bm, fancyhdr, graphicx, geometry, mathrsfs, zhnumber, url} 3 | \usepackage{tcolorbox, booktabs, listings, accsupp} 4 | \usepackage[bookmarks=true, colorlinks, citecolor=black, linkcolor=black]{hyperref} 5 | \usepackage[ruled]{algorithm2e} 6 | % \usepackage[framed, numbered, autolinebreaks, useliterate]{mcode} 7 | 8 | \linespread{1.2} 9 | \geometry{left=2.5cm, right=2.5cm, top=2.5cm, bottom=2.5cm} 10 | \ctexset {chapter={name={第\,,\,章},number={\arabic{chapter}}}} 11 | \renewcommand{\thesection}{\arabic{section}.} 12 | \renewcommand{\baselinestretch}{1.0} 13 | \renewcommand{\thesubsection}{\arabic{section}.\arabic{subsection}} 14 | \renewcommand{\theequation}{\arabic{section}.\arabic{equation}} 15 | \renewcommand{\thetable}{\arabic{section}.\arabic{table}} 16 | \renewcommand{\thefigure}{\arabic{section}.\arabic{figure}} 17 | 18 | \newcommand{\ud}{\mathop{}\negthinspace\mathrm{d}} 19 | 20 | % 代码环境 21 | \usepackage{listings} 22 | % 复制代码时不复制行号 23 | \usepackage{accsupp} 24 | \newcommand{\emptyaccsupp}[1]{\BeginAccSupp{ActualText={}}#1\EndAccSupp{}} 25 | \usepackage{tcolorbox} 26 | \tcbuselibrary{listings,skins,breakable,xparse} 27 | 28 | % global style 29 | \lstset{ 30 | basicstyle=\small\ttfamily, 31 | % Word styles 32 | keywordstyle=\color{blue}, 33 | commentstyle=\color{green!50!black}, 34 | columns=fullflexible, % Avoid too sparse word spaces 35 | keepspaces=true, 36 | % Numbering 37 | numbers=left, 38 | numberstyle=\color{red!75!black}\emptyaccsupp, 39 | % Lines and Skips 40 | aboveskip=0pt plus 6pt, 41 | belowskip=0pt plus 6pt, 42 | breaklines=true, 43 | breakatwhitespace=true, 44 | emptylines=1} 45 | 46 | \graphicspath{{./graphics/}} 47 | 48 | \begin{document} 49 | 50 | \pagestyle{empty} 51 | \setcounter{page}{0} 52 | 53 | \begin{center} 54 | \Large{\textbf{\LaTeX 模板3.0}} 55 | \end{center} 56 | 57 | \begin{center} 58 | \Large{\textbf{摘要}} 59 | \end{center} 60 | 61 | 这里是摘要。 62 | 63 | \textbf{关键词：}这里是关键词；这里是关键词。 64 | 65 | \newpage 66 | \setcounter{page}{1} 67 | \pagenumbering{arabic} 68 | \pagestyle{plain} 69 | \fancyfoot[C]{\thepage} 70 | 71 | \input{chapter1.tex} 72 | 73 | \input{chapter4.tex} 74 | 75 | \newpage 76 | \begin{center} 77 | \Large{\textbf{附录}} 78 | \end{center} 79 | 80 | \begin{appendices} 81 | \renewcommand{\thesection}{\Alph{section}} 82 | \section{所用软件} 83 | 论文使用\LaTeX 排版。 84 | \section{代码} 85 | 所使用的代码如下。 86 | % \begin{lstlisting} 87 | % Hello. 88 | % \end{lstlisting} 89 | \end{appendices} 90 | 91 | \end{document} -------------------------------------------------------------------------------- /include/asmext.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VVERY_SIMPLE_ASMEXT_HEADER 2 | #define VVERY_SIMPLE_ASMEXT_HEADER 3 | 4 | #if defined(PPX_USE_AVX) 5 | #include 6 | #endif 7 | #include 8 | 9 | namespace ppx 10 | { 11 | #if defined(PPX_USE_AVX) 12 | namespace avxt 13 | { 14 | // avx related 15 | template 16 | bool chkalign(const T *p, size_t alignment) 17 | { 18 | if (p == nullptr) 19 | { 20 | return false; 21 | } 22 | if (((uintptr_t)p % alignment) != 0) 23 | { 24 | return false; 25 | } 26 | return true; 27 | } 28 | 29 | inline double sum(const double *start, size_t n) 30 | { 31 | auto q_sum = _mm256_setzero_pd(); 32 | size_t i = 0; 33 | for (; n - i >= 4; i += 4) 34 | { 35 | q_sum = _mm256_add_pd(_mm256_loadu_pd(start + i), q_sum); 36 | } 37 | // Perform reduction, final sum in low-order element of temp3 38 | auto temp = _mm_add_pd(_mm256_extractf128_pd(q_sum, 0), 39 | _mm256_extractf128_pd(q_sum, 1)); 40 | // Process remaining elements 41 | double sum{}; 42 | _mm_store_sd(&sum, _mm_hadd_pd(temp, temp)); 43 | 44 | for (; i < n; i++) 45 | { 46 | sum += *(start + i); 47 | } 48 | return sum; 49 | } 50 | 51 | inline double inrpdt(const double *a, const double *b, size_t n) 52 | { 53 | auto q_sum = _mm256_setzero_pd(); 54 | size_t i = 0; 55 | for (; n - i >= 4; i += 4) 56 | { 57 | auto q_a = _mm256_loadu_pd(a + i); 58 | auto q_b = _mm256_loadu_pd(b + i); 59 | q_sum = _mm256_fmadd_pd(q_a, q_b, q_sum); 60 | } 61 | // Peform reduction, final sum in low-order element of temp3 62 | auto temp = _mm_add_pd(_mm256_extractf128_pd(q_sum, 0), 63 | _mm256_extractf128_pd(q_sum, 1)); 64 | // Process remaining elements 65 | double sum{}; 66 | _mm_store_sd(&sum, _mm_hadd_pd(temp, temp)); 67 | 68 | for (; i < n; i++) 69 | { 70 | auto na = *(a + i); 71 | auto nb = *(b + i); 72 | sum += na * nb; 73 | } 74 | return sum; 75 | } 76 | 77 | template 78 | inline void gemm(const double *a, const double *b, double *c) 79 | { 80 | constexpr uint64_t ZR = 0; 81 | constexpr uint64_t MV = 0x8000000000000000; 82 | alignas(32) const uint64_t c_Mask0[4]{ZR, ZR, ZR, ZR}; 83 | alignas(32) const uint64_t c_Mask1[4]{MV, ZR, ZR, ZR}; 84 | alignas(32) const uint64_t c_Mask2[4]{MV, MV, ZR, ZR}; 85 | alignas(32) const uint64_t c_Mask3[4]{MV, MV, MV, ZR}; 86 | 87 | const uint64_t *c_MaskMovLUT[8]{c_Mask0, c_Mask1, c_Mask2, c_Mask3}; 88 | constexpr size_t left_cols = M % 4; 89 | auto res_mask = _mm256_load_si256((__m256i *)c_MaskMovLUT[left_cols]); 90 | 91 | for (size_t j = 0; j < L; j++) 92 | { 93 | size_t i = 0; 94 | while (i + 4 <= M) 95 | { 96 | auto quad_c = _mm256_setzero_pd(); 97 | for (size_t k = 0; k < N; k++) 98 | { 99 | quad_c = _mm256_fmadd_pd(_mm256_loadu_pd(a + i + k * M), 100 | _mm256_broadcast_sd(b + k + j * N), 101 | quad_c); 102 | } 103 | _mm256_storeu_pd(c + i + j * M, quad_c); 104 | i += 4; 105 | } 106 | if (left_cols) 107 | { 108 | auto quad_c = _mm256_setzero_pd(); 109 | for (size_t k = 0; k < N; k++) 110 | { 111 | quad_c = _mm256_fmadd_pd(_mm256_maskload_pd(a + i + k * M, res_mask), 112 | _mm256_broadcast_sd(b + k + j * N), 113 | quad_c); 114 | } 115 | _mm256_maskstore_pd(c + i + j * M, res_mask, quad_c); 116 | } 117 | } 118 | } 119 | 120 | inline void transpose4x4(const double *a, double *b) 121 | { 122 | auto q_a = _mm256_loadu_pd(a); 123 | auto q_b = _mm256_loadu_pd(a + 4); 124 | auto q_c = _mm256_loadu_pd(a + 8); 125 | auto q_d = _mm256_loadu_pd(a + 12); 126 | 127 | auto temp0 = _mm256_unpacklo_pd(q_a, q_b); 128 | auto temp1 = _mm256_unpackhi_pd(q_a, q_b); 129 | auto temp2 = _mm256_unpacklo_pd(q_c, q_d); 130 | auto temp3 = _mm256_unpackhi_pd(q_c, q_d); 131 | 132 | _mm256_storeu_pd(b, _mm256_permute2f128_pd(temp0, temp2, 0x20)); 133 | _mm256_storeu_pd(b + 4, _mm256_permute2f128_pd(temp1, temp3, 0x20)); 134 | _mm256_storeu_pd(b + 8, _mm256_permute2f128_pd(temp0, temp2, 0x31)); 135 | _mm256_storeu_pd(b + 12, _mm256_permute2f128_pd(temp1, temp3, 0x31)); 136 | } 137 | } 138 | #endif 139 | } 140 | #endif -------------------------------------------------------------------------------- /include/liegroup.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VVERY_SIMPLE_LIEGROUP_HEADER 2 | #define VVERY_SIMPLE_LIEGROUP_HEADER 3 | 4 | #include "statistics.hpp" 5 | 6 | namespace ppx 7 | { 8 | using so3 = MatrixS<3, 1>; 9 | using se3 = MatrixS<6, 1>; 10 | using T3 = MatrixS<3, 1>; 11 | 12 | inline MatrixS<3, 3> hat(const so3 &vec) 13 | { 14 | return {0.0, vec[2], -vec[1], -vec[2], 0.0, vec[0], vec[1], -vec[0], 0.0}; 15 | } 16 | 17 | inline so3 vee(const MatrixS<3, 3> &mat) 18 | { 19 | return {mat[5], mat[6], mat[1]}; 20 | } 21 | 22 | inline MatrixS<4, 4> hat(const se3 &vec) 23 | { 24 | return {0.0, vec[2], -vec[1], 0.0, -vec[2], 0.0, vec[0], 0.0, vec[1], -vec[0], 0.0, 0.0, vec[3], vec[4], vec[5], 0.0}; 25 | } 26 | 27 | inline se3 vee(const MatrixS<4, 4> &mat) 28 | { 29 | return {mat[6], mat[8], mat[1], mat[12], mat[13], mat[14]}; 30 | } 31 | 32 | class SO3 : public MatrixS<3, 3> 33 | { 34 | using Rep = MatrixS<3, 3>; 35 | 36 | public: 37 | template * = nullptr> 38 | SO3(std::initializer_list list) : MatrixS(list) {} 39 | SO3(const Rep &other) : MatrixS(other) {} 40 | SO3() : MatrixS(eye<3>()){}; 41 | SO3(const MatrixS<3, 1> &xAxis, const MatrixS<3, 1> &yAxis, const MatrixS<3, 1> &zAxis) 42 | : MatrixS<3, 3>{xAxis[0], xAxis[1], xAxis[2], yAxis[0], yAxis[1], yAxis[2], zAxis[0], zAxis[1], zAxis[2]} {} 43 | SO3(double m[3][3]) : MatrixS({m[0][0], m[1][0], m[2][0], m[0][1], m[1][1], m[2][1], m[0][2], m[1][2], m[2][2]}) {} 44 | 45 | using MatrixS::operator*; 46 | 47 | SO3 operator*(const SO3 &other) const 48 | { 49 | return MatrixS::operator*(other); 50 | } 51 | 52 | MatrixS<3, 3> Adt() const 53 | { 54 | return *this; 55 | } 56 | SO3 I() const 57 | { 58 | return this->T(); 59 | } 60 | so3 log() const 61 | { 62 | const auto &R = *this; 63 | auto acosinput = (this->trace() - 1) / 2.0; 64 | if (acosinput >= 1.0) 65 | { 66 | return {}; 67 | } 68 | else if (acosinput <= -1.0) 69 | { 70 | MatrixS<3, 1> omg{}; 71 | if (!details::is_same(1 + R(2, 2), 0.0)) 72 | { 73 | omg = (1.0 / sqrt(2 + 2 * R(2, 2))) * T3{R(0, 2), R(1, 2), 1 + R(2, 2)}; 74 | } 75 | else if (!details::is_same(1 + R(1, 1), 0.0)) 76 | { 77 | omg = (1.0 / sqrt(2 + 2 * R(1, 1))) * T3{R(0, 1), 1 + R(1, 1), +R(2, 1)}; 78 | } 79 | else 80 | { 81 | omg = (1.0 / sqrt(2 + 2 * R(0, 0))) * T3{1 + R(0, 0), R(1, 0), +R(2, 0)}; 82 | } 83 | return PI * omg; 84 | } 85 | else 86 | { 87 | MatrixS<3, 3> ret{}; 88 | double theta = acos(acosinput); 89 | ret = (R - R.T()) * theta / (2.0 * sin(theta)); 90 | return vee(ret); 91 | } 92 | } 93 | 94 | static SO3 RotX(double theta) 95 | { 96 | return {1.0, 0.0, 0.0, 97 | 0.0, cos(theta), -sin(theta), 98 | 0.0, sin(theta), cos(theta)}; 99 | } 100 | 101 | static SO3 RotY(double theta) 102 | { 103 | return {cos(theta), 0.0, -sin(theta), 104 | 0.0, 1.0, 0.0, 105 | sin(theta), 0.0, cos(theta)}; 106 | } 107 | 108 | static SO3 RotZ(double theta) 109 | { 110 | return {cos(theta), sin(theta), 0.0, 111 | -sin(theta), cos(theta), 0.0, 112 | 0.0, 0.0, 1.0}; 113 | } 114 | }; 115 | 116 | template 117 | template *> 118 | SO3 MatrixS::exp() const 119 | { 120 | const auto &s = *this; 121 | double theta = norm2(s); 122 | if (details::near_zero(theta)) 123 | { 124 | return {}; 125 | } 126 | else 127 | { 128 | auto omg = hat(s); 129 | auto coff1 = sin(theta) / theta; 130 | auto coff2 = (1 - cos(theta)) / (theta * theta); 131 | return eye<3>() + coff1 * omg + coff2 * (omg * omg); 132 | } 133 | } 134 | 135 | template 136 | template *> 137 | MatrixS<3, 3> MatrixS::adt() const 138 | { 139 | return hat(*this); 140 | } 141 | 142 | template 143 | template *> 144 | MatrixS<3, 3> MatrixS::ljac() const 145 | { 146 | auto result = eye<3>(); 147 | auto x2 = this->m_data[0] * this->m_data[0] + this->m_data[1] * this->m_data[1] + this->m_data[2] * this->m_data[2]; 148 | auto X = hat(*this); 149 | if (x2 < EPS_DP) 150 | { 151 | return result + 0.5 * X; 152 | } 153 | auto x = sqrt(x2); 154 | return result + (1 - cos(x)) / x2 * X + (x - sin(x)) / (x2 * x) * X * X; 155 | } 156 | 157 | template 158 | template *> 159 | MatrixS<3, 3> MatrixS::ljacinv() const 160 | { 161 | auto result = eye<3>(); 162 | auto x2 = this->m_data[0] * this->m_data[0] + this->m_data[1] * this->m_data[1] + this->m_data[2] * this->m_data[2]; 163 | auto X = hat(*this); 164 | if (x2 < EPS_DP) 165 | { 166 | return result - 0.5 * X; 167 | } 168 | auto x = sqrt(x2); 169 | return result - 0.5 * X + (1 / x2 - (1 + cos(x)) / (2 * x * sin(x))) * X * X; 170 | } 171 | 172 | template 173 | template *> 174 | MatrixS<3, 3> MatrixS::rjac() const 175 | { 176 | return ljac().T(); 177 | } 178 | 179 | template 180 | template *> 181 | MatrixS<3, 3> MatrixS::rjacinv() const 182 | { 183 | return ljacinv().T(); 184 | } 185 | 186 | class SE3 : public MatrixS<4, 4> 187 | { 188 | using Rep = MatrixS<4, 4>; 189 | 190 | public: 191 | template * = nullptr> 192 | SE3(std::initializer_list list) : MatrixS(list) {} 193 | SE3(const Rep &other) : MatrixS(other) {} 194 | SE3(const SO3 &Rot, const T3 &Pos) : MatrixS(eye<4>()) 195 | { 196 | (*this).sub<3, 3, false>(0, 0) = Rot; 197 | (*this).sub<3, 1, false>(0, 3) = Pos; 198 | } 199 | SE3() : MatrixS(eye<4>()) {} 200 | SE3(double m[4][4]) : MatrixS({m[0][0], m[1][0], m[2][0], 0.0, m[0][1], m[1][1], m[2][1], 0.0, m[0][2], m[1][2], m[2][2], 0.0, m[0][3], m[1][3], m[2][3], 1.0}) {} 201 | 202 | using MatrixS::operator*; 203 | 204 | SE3 operator*(const SE3 &other) const 205 | { 206 | return MatrixS::operator*(other); 207 | } 208 | 209 | SO3 Rot() const 210 | { 211 | return (*this).sub<3, 3>(0, 0); 212 | } 213 | T3 Pos() const 214 | { 215 | return (*this).sub<3, 1>(0, 3); 216 | } 217 | MatrixS<6, 6> Adt() const 218 | { 219 | MatrixS<6, 6> result{}; 220 | const auto &R = Rot(); 221 | const auto &p = Pos(); 222 | result.sub<3, 3, false>(0, 0) = R; 223 | result.sub<3, 3, false>(3, 0) = hat(p) * R; 224 | result.sub<3, 3, false>(3, 3) = R; 225 | return result; 226 | } 227 | SE3 I() const 228 | { 229 | return {Rot().T(), -1 * (Rot().T() * Pos())}; 230 | } 231 | se3 log() const 232 | { 233 | const auto &R = Rot(); 234 | const auto &p = Pos(); 235 | auto ctheta = (R.trace() - 1) / 2.0; 236 | assert(fabs(ctheta) < 1 + EPS_SP); 237 | if (fabs(ctheta) < 1) 238 | { 239 | auto theta = acos(ctheta); 240 | auto omg = R.log(); 241 | auto omgmat = hat(omg); 242 | auto coff1 = (1.0 / theta - 1.0 / (tan(theta / 2.0) * 2.0)) / theta; 243 | MatrixS<3, 3> logExpand = eye<3>() - omgmat / 2.0 + coff1 * (omgmat * omgmat); 244 | return {omg, logExpand * p}; 245 | } 246 | else 247 | { 248 | return {T3{}, p}; 249 | } 250 | } 251 | }; 252 | 253 | template 254 | template *> 255 | SE3 MatrixS::exp() const 256 | { 257 | auto phi = norm2(_1()); 258 | if (details::is_same(phi, 0)) 259 | { 260 | return {SO3(), _2()}; 261 | } 262 | else 263 | { 264 | const auto ksi = hat(*this); 265 | auto coff1 = (1 - cos(phi)) / (phi * phi); 266 | auto coff2 = (phi - sin(phi)) / (phi * phi * phi); 267 | return eye<4>() + ksi + coff1 * (ksi * ksi) + coff2 * (ksi * ksi * ksi); 268 | } 269 | } 270 | 271 | template 272 | template *> 273 | MatrixS<6, 6> MatrixS::adt() const 274 | { 275 | MatrixS<6, 6> result; 276 | result.sub<3, 3, false>(0, 0) = hat(_1()); 277 | result.sub<3, 3, false>(3, 0) = hat(_2()); 278 | result.sub<3, 3, false>(3, 3) = hat(_1()); 279 | return result; 280 | } 281 | 282 | } 283 | #endif -------------------------------------------------------------------------------- /include/statistics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VVERY_SIMPLE_STATISTICS_HEADER 2 | #define VVERY_SIMPLE_STATISTICS_HEADER 3 | 4 | #include "optimization.hpp" 5 | #include 6 | #include 7 | 8 | namespace ppx 9 | { 10 | // Moments of a Distribution: Mean, Variance, Skewness 11 | template 12 | auto avg(const T &vec) 13 | { 14 | using value_t = typename T::value_type; 15 | return static_cast(std::accumulate(vec.begin(), vec.end(), value_t{}) / vec.size()); 16 | } 17 | 18 | template 19 | double var(const T &vec) 20 | { 21 | assert(vec.size() >= 1); 22 | using value_t = typename T::value_type; 23 | value_t e = avg(vec); 24 | double sum = 0.0; 25 | for (size_t i = 0; i < vec.size(); i++) 26 | { 27 | value_t tmp = vec[i] - e; 28 | sum += tmp * tmp; 29 | } 30 | return sum / (vec.size() - 1); 31 | } 32 | 33 | template 34 | void random(MatrixS &mat, double lo = -MAX_SP, double hi = MAX_SP) 35 | { 36 | std::random_device rd; 37 | std::mt19937 gen(rd()); 38 | std::uniform_real_distribution<> distr(lo, hi); 39 | for (auto &i : mat) 40 | { 41 | i = distr(gen); 42 | } 43 | } 44 | 45 | template 46 | class MultiGaussianDistribution 47 | { 48 | public: 49 | using samples = std::vector>; 50 | 51 | MultiGaussianDistribution() : m_cov(eye()), m_gen(std::random_device{}()) {} 52 | 53 | MultiGaussianDistribution(const MatrixS &mu, const MatrixS &sigma) 54 | : m_mean(mu), m_cov(sigma), m_gen(std::random_device{}()) {} 55 | 56 | MatrixS operator()() const 57 | { 58 | MatrixS x; 59 | std::normal_distribution<> d{0, 1}; 60 | EigenValue eigsys(m_cov); 61 | MatrixS diag; 62 | for (size_t i = 0; i < N; i++) 63 | { 64 | x[i] = d(m_gen); 65 | diag(i, i) = sqrt(eigsys.d[i]); 66 | } 67 | return eigsys.z * diag * x + m_mean; 68 | } 69 | 70 | double pdf(const MatrixS &x) const 71 | { 72 | int n = static_cast(N); 73 | MatrixS normalized_mu = x - m_mean; 74 | // double quadform = (normalized_mu.T() * m_cov.I() * normalized_mu)[0]; 75 | double quadform = inner_product(normalized_mu, normalized_mu, m_cov.I()); 76 | double norm = std::pow(std::sqrt(2 * PI), -n) / std::sqrt(m_cov.det()); 77 | return norm * std::exp(-0.5 * quadform); 78 | } 79 | 80 | const MatrixS &mean() const 81 | { 82 | return m_mean; 83 | } 84 | 85 | MatrixS &mean() 86 | { 87 | return m_mean; 88 | } 89 | 90 | const MatrixS &covariance() const 91 | { 92 | return m_cov; 93 | } 94 | 95 | MatrixS &covariance() 96 | { 97 | return m_cov; 98 | } 99 | 100 | void fit(const samples &data) 101 | { 102 | auto n = data.size(); 103 | auto sum_m = std::accumulate(data.begin(), data.end(), MatrixS()); 104 | m_mean = sum_m / n; 105 | MatrixS sum_s; 106 | for (size_t i = 0; i < n; i++) 107 | { 108 | MatrixS tpx = data.at(i) - m_mean; 109 | sum_s += tpx * tpx.T(); 110 | } 111 | m_cov = sum_s / n; 112 | } 113 | 114 | private: 115 | MatrixS m_mean; 116 | MatrixS m_cov; 117 | mutable std::mt19937 m_gen; 118 | }; 119 | 120 | template 121 | using MVN = MultiGaussianDistribution; 122 | 123 | template 124 | std::ostream &operator<<(std::ostream &os, const MultiGaussianDistribution &self) 125 | { 126 | os << "MultiNormalDistribution<" << N << ">:\n" 127 | << "mean = \t" << self.mean() << "\n" 128 | << "cov = \t" << self.covariance(); 129 | return os; 130 | } 131 | 132 | template 133 | class MixedGaussianDistribution 134 | { 135 | public: 136 | using dist = MultiGaussianDistribution; 137 | using samples = std::vector>; 138 | 139 | MixedGaussianDistribution() : m_prior(), m_gen(std::random_device{}()) {} 140 | 141 | double pdf(const MatrixS &x) 142 | { 143 | double sum = 0.0; 144 | for (size_t i = 0; i < K; i++) 145 | { 146 | sum += m_guassian[i].pdf(x) * m_prior[i]; 147 | } 148 | return sum; 149 | } 150 | 151 | MatrixS operator()() const 152 | { 153 | std::uniform_real_distribution<> dis(0.0, 1.0); 154 | double sample = dis(m_gen); 155 | double sum = m_prior.front(); 156 | size_t idx = 0; 157 | while (sample > sum && idx < K) 158 | { 159 | sum += m_prior[idx]; 160 | ++idx; 161 | } 162 | return m_guassian[idx](); 163 | } 164 | 165 | const dist &distribution(size_t idx) const 166 | { 167 | return m_guassian.at(idx); 168 | } 169 | 170 | dist &distribution(size_t idx) 171 | { 172 | return m_guassian.at(idx); 173 | } 174 | 175 | double prior(size_t idx) const 176 | { 177 | return m_prior.at(idx); 178 | } 179 | 180 | void setcomp(size_t idx, const dist &d, double p) 181 | { 182 | m_guassian[idx] = d; 183 | m_prior[idx] = p; 184 | } 185 | 186 | void fit(const samples &data) 187 | { 188 | constexpr auto c = K; 189 | auto n = data.size(); 190 | double residual = 1.0; 191 | double last_p = 0.0; 192 | auto its = 0u; 193 | 194 | while (residual > EPS_SP && its < ITMAX) 195 | { 196 | std::vector> a(c, std::vector(n)); 197 | for (size_t k = 0; k < c; k++) 198 | { 199 | std::transform(data.begin(), data.end(), a[k].begin(), 200 | [&](const MatrixS &x) 201 | { return m_prior[k] * m_guassian[k].pdf(x) / pdf(x); }); 202 | } 203 | for (size_t k = 0; k < c; k++) 204 | { 205 | auto sum_g = std::accumulate(a[k].begin(), a[k].end(), 0.0); 206 | m_prior[k] = sum_g / n; 207 | MatrixS sum_m = std::inner_product(a[k].begin(), a[k].end(), data.begin(), MatrixS()); 208 | sum_m /= sum_g; 209 | MatrixS sum_s; 210 | for (size_t i = 0; i < n; i++) 211 | { 212 | MatrixS tpx = data.at(i) - sum_m; 213 | sum_s += tpx * tpx.T() * a[k][i]; 214 | } 215 | m_guassian[k].mean() = sum_m; 216 | m_guassian[k].covariance() = sum_s / sum_g; 217 | } 218 | double tpp = std::accumulate(data.begin(), data.end(), 0.0, 219 | [&](double y0, const MatrixS &x) 220 | { return y0 + std::log(pdf(x)); }); 221 | residual = fabs(last_p - tpp); 222 | last_p = tpp; 223 | ++its; 224 | } 225 | } 226 | 227 | template 228 | std::enable_if_t<(L < N), std::vector>> 229 | predict(std::vector> &known, const std::array &idx) const 230 | { 231 | std::array xory; 232 | for (size_t i = 0; i < N; i++) 233 | { 234 | xory[i] = std::any_of(idx.cbegin(), idx.cend(), [i](size_t a) 235 | { return a == i; }); 236 | } 237 | 238 | std::vector> result; 239 | std::vector> u_x_k; 240 | std::vector> cov_xx_k; 241 | std::vector> u_y_k; 242 | std::vector> reg_k; 243 | 244 | for (size_t k = 0; k < K; k++) 245 | { 246 | const auto &u_total = m_guassian[k].mean(); 247 | const auto &cov_total = m_guassian[k].covariance(); 248 | MatrixS u_x; 249 | MatrixS u_y; 250 | MatrixS cov_xx; 251 | MatrixS cov_yx; 252 | 253 | size_t idx1 = 0; 254 | size_t idx2 = 0; 255 | for (size_t i = 0; i < N; i++) 256 | { 257 | if (xory[i]) 258 | { 259 | u_x[idx1] = u_total[i]; 260 | size_t jdx1 = 0; 261 | for (size_t j = 0; j < N; j++) 262 | { 263 | if (xory[j]) 264 | { 265 | cov_xx(idx1, jdx1) = cov_total(i, j); 266 | jdx1++; 267 | } 268 | } 269 | idx1++; 270 | } 271 | else 272 | { 273 | u_y[idx2] = u_total[i]; 274 | size_t jdx2 = 0; 275 | for (size_t j = 0; j < N; j++) 276 | { 277 | if (xory[j]) 278 | { 279 | cov_yx(idx2, jdx2) = cov_total(i, j); 280 | jdx2++; 281 | } 282 | } 283 | idx2++; 284 | } 285 | } 286 | u_x_k.push_back(std::move(u_x)); 287 | u_y_k.push_back(std::move(u_y)); 288 | reg_k.push_back(cov_yx * cov_xx.I()); 289 | cov_xx_k.push_back(std::move(cov_xx)); 290 | } 291 | 292 | for (const auto &elem : known) 293 | { 294 | MatrixS posterior_coff{}; 295 | for (size_t i = 0; i < K; i++) 296 | { 297 | MVN marginal_gmm(u_x_k[i], cov_xx_k[i]); 298 | posterior_coff[i] = m_prior[i] * marginal_gmm.pdf(elem); 299 | } 300 | posterior_coff /= sum(posterior_coff.data(), K); 301 | 302 | MatrixS tmp; 303 | for (size_t i = 0; i < K; i++) 304 | { 305 | MatrixS u = u_y_k[i] + reg_k[i] * (elem - u_x_k[i]); 306 | tmp = tmp + posterior_coff[i] * u; 307 | } 308 | result.push_back(std::move(tmp)); 309 | } 310 | 311 | return result; 312 | } 313 | 314 | private: 315 | std::array m_guassian; 316 | std::array m_prior; 317 | mutable std::mt19937 m_gen; 318 | size_t ITMAX = 200; 319 | }; 320 | 321 | template 322 | std::ostream &operator<<(std::ostream &os, const MixedGaussianDistribution &self) 323 | { 324 | os << "MixedNormalDistribution<" << N << ',' << K << ">:\n"; 325 | for (size_t i = 0; i < K; i++) 326 | { 327 | os << self.prior(i) << " of all, component " << i << ":\n"; 328 | os << self.distribution(i) << "\n"; 329 | } 330 | return os; 331 | } 332 | 333 | template 334 | using GMM = MixedGaussianDistribution; 335 | } 336 | #endif -------------------------------------------------------------------------------- /log/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(plog plog.cpp) 2 | target_include_directories(plog PUBLIC ./) 3 | if(CMAKE_SYSTEM_NAME MATCHES "Linux") 4 | elseif(CMAKE_SYSTEM_NAME MATCHES "Windows") 5 | target_compile_definitions(plog PRIVATE _CRT_SECURE_NO_WARNINGS) 6 | target_link_libraries(plog PRIVATE wsock32 ws2_32) 7 | endif() -------------------------------------------------------------------------------- /log/plog.h: -------------------------------------------------------------------------------- 1 | #ifndef VVERY_SIMPLE_PPXLOG_HEADER 2 | #define VVERY_SIMPLE_PPXLOG_HEADER 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // Operating System Evaluation 10 | #if (defined(_WIN32) || defined(_WIN64)) 11 | #define PLOG_OS_WINDOWS 1 12 | #else 13 | #define PLOG_OS_WINDOWS 0 14 | #endif 15 | #if (defined(__linux) || defined(__linux__)) 16 | #define PLOG_OS_LINUX 1 17 | #else 18 | #define PLOG_OS_LINUX 0 19 | #endif 20 | #if (defined(__QNX__) || defined(__QNXNTO__)) 21 | #define PLOG_OS_QNX 1 22 | #else 23 | #define PLOG_OS_QNX 0 24 | #endif 25 | #if (defined(__APPLE__)) 26 | #define PLOG_OS_MAC 1 27 | #else 28 | #define PLOG_OS_MAC 0 29 | #endif 30 | // above are *nix 31 | #if ((PLOG_OS_LINUX || PLOG_OS_MAC || PLOG_OS_QNX) && (!PLOG_OS_WINDOWS)) 32 | #define PLOG_OS_UNIX 1 33 | #else 34 | #define PLOG_OS_UNIX 0 35 | #endif 36 | // code are not guaranteed except these OSs. let it makes compiler errors. 37 | #if (!(PLOG_OS_UNIX || PLOG_OS_WINDOWS)) 38 | #error unknow Operating system for PLOG! 39 | #endif 40 | 41 | // Helper macro for declaring functions as having similar signature to printf. 42 | // This allows the compiler to catch format errors at compile-time. 43 | #if defined(__clang__) || defined(__GNUC__) 44 | #define PLOG_PRINTF_CHECK(fmtarg, firstvararg) __attribute__((__format__(__printf__, fmtarg, firstvararg))) 45 | #define PLOG_PRINT_STRING_TYPE const char * 46 | #elif defined(_MSC_VER) 47 | #define PLOG_PRINTF_CHECK(fmtarg, firstvararg) 48 | #define PLOG_PRINT_STRING_TYPE _In_z_ _Printf_format_string_ const char * 49 | #else 50 | #define PLOG_PRINTF_CHECK(fmtarg, firstvararg) 51 | #define PLOG_PRINT_STRING_TYPE const char * 52 | #endif 53 | 54 | namespace ppx 55 | { 56 | namespace details 57 | { 58 | template 59 | struct make_noid 60 | { 61 | typedef void type; 62 | }; 63 | 64 | template 65 | using noid_t = typename make_noid::type; 66 | 67 | template > 68 | struct is_overloaded_stream_impl : public std::false_type 69 | { 70 | }; 71 | 72 | template 73 | struct is_overloaded_stream_impl())>> : public std::true_type 74 | { 75 | }; 76 | 77 | template 78 | static constexpr bool is_overloaded_stream() 79 | { 80 | return is_overloaded_stream_impl::value; 81 | } 82 | 83 | void psuedo_log_flush(); 84 | } 85 | 86 | inline const char *filename(const char *path) 87 | { 88 | for (auto ptr = path; *ptr; ++ptr) 89 | { 90 | if (*ptr == '/' || *ptr == '\\') 91 | { 92 | path = ptr + 1; 93 | } 94 | } 95 | return path; 96 | } 97 | 98 | inline const char *funcname(const char *func) 99 | { 100 | for (auto ptr = func + 1; *ptr; ++ptr) 101 | { 102 | if (*ptr == ':' && *(ptr - 1) == ':') 103 | { 104 | func = ptr + 1; 105 | } 106 | } 107 | return func; 108 | } 109 | 110 | struct bit_t 111 | { 112 | explicit constexpr bit_t(int b) : m_bit_idx(b) {} 113 | explicit constexpr operator int() const { return m_bit_idx; } 114 | 115 | private: 116 | int m_bit_idx; 117 | }; 118 | 119 | constexpr bit_t operator"" _bit(unsigned long long int b) { return bit_t{static_cast(b)}; } 120 | 121 | template ::value>::type> 122 | struct bit_flag 123 | { 124 | constexpr bit_flag(bit_flag const &rhs) noexcept = default; 125 | constexpr bit_flag(bit_flag &&rhs) noexcept = default; 126 | constexpr bit_flag() noexcept : m_val(0) {} 127 | explicit constexpr bit_flag(T const val) noexcept : m_val(val) {} 128 | constexpr bit_flag(bit_t const bit) noexcept : m_val(static_cast(T{1} << static_cast(bit))) {} 129 | explicit constexpr operator T() const noexcept { return m_val; } 130 | explicit constexpr operator bool() const noexcept { return m_val != 0; } 131 | 132 | static constexpr bit_flag all() 133 | { 134 | return bit_flag(~T{0}); 135 | } 136 | 137 | T val() const 138 | { 139 | return m_val; 140 | } 141 | 142 | bool constexpr operator==(bit_flag const f) const noexcept 143 | { 144 | return m_val == f.m_val; 145 | } 146 | 147 | bool constexpr operator!=(bit_flag const f) const noexcept 148 | { 149 | return m_val != f.m_val; 150 | } 151 | 152 | bit_flag &operator|=(bit_flag const f) noexcept 153 | { 154 | m_val |= f.m_val; 155 | return *this; 156 | } 157 | 158 | bit_flag &operator&=(bit_flag const f) noexcept 159 | { 160 | m_val &= f.m_val; 161 | return *this; 162 | } 163 | 164 | bit_flag &operator^=(bit_flag const f) noexcept 165 | { 166 | m_val ^= f.m_val; 167 | return *this; 168 | } 169 | 170 | constexpr friend bit_flag operator|(bit_flag const lhs, bit_flag const rhs) noexcept 171 | { 172 | return bit_flag(lhs.m_val | rhs.m_val); 173 | } 174 | 175 | constexpr friend bit_flag operator&(bit_flag const lhs, bit_flag const rhs) noexcept 176 | { 177 | return bit_flag(lhs.m_val & rhs.m_val); 178 | } 179 | 180 | constexpr friend bit_flag operator^(bit_flag const lhs, bit_flag const rhs) noexcept 181 | { 182 | return bit_flag(lhs.m_val ^ rhs.m_val); 183 | } 184 | 185 | constexpr bit_flag operator~() const noexcept 186 | { 187 | return bit_flag(~m_val); 188 | } 189 | 190 | bit_flag &operator=(bit_flag const &rhs) noexcept = default; 191 | bit_flag &operator=(bit_flag &&rhs) noexcept = default; 192 | 193 | private: 194 | T m_val; 195 | }; 196 | 197 | using LogLevel = bit_flag; 198 | 199 | constexpr LogLevel CH000 = 0_bit; 200 | constexpr LogLevel CH001 = 1_bit; 201 | constexpr LogLevel CH002 = 2_bit; 202 | constexpr LogLevel CH003 = 3_bit; 203 | constexpr LogLevel CH010 = 4_bit; 204 | constexpr LogLevel CH011 = CH001 | CH010; 205 | constexpr LogLevel CH012 = CH002 | CH010; 206 | constexpr LogLevel CH013 = CH003 | CH010; 207 | constexpr LogLevel CH020 = 5_bit; 208 | constexpr LogLevel CH021 = CH001 | CH020; 209 | constexpr LogLevel CH022 = CH002 | CH020; 210 | constexpr LogLevel CH023 = CH003 | CH020; 211 | constexpr LogLevel CH030 = 6_bit; 212 | constexpr LogLevel CH031 = CH001 | CH030; 213 | constexpr LogLevel CH032 = CH002 | CH030; 214 | constexpr LogLevel CH033 = CH003 | CH030; 215 | constexpr LogLevel CH100 = 7_bit; 216 | constexpr LogLevel CH101 = CH001 | CH100; 217 | constexpr LogLevel CH102 = CH002 | CH100; 218 | constexpr LogLevel CH103 = CH003 | CH100; 219 | constexpr LogLevel CH110 = CH010 | CH100; 220 | constexpr LogLevel CH111 = CH011 | CH100; 221 | constexpr LogLevel CH112 = CH012 | CH100; 222 | constexpr LogLevel CH113 = CH013 | CH100; 223 | constexpr LogLevel CH120 = CH020 | CH100; 224 | constexpr LogLevel CH121 = CH021 | CH100; 225 | constexpr LogLevel CH122 = CH022 | CH100; 226 | constexpr LogLevel CH123 = CH023 | CH100; 227 | constexpr LogLevel CH130 = CH030 | CH100; 228 | constexpr LogLevel CH131 = CH031 | CH100; 229 | constexpr LogLevel CH132 = CH032 | CH100; 230 | constexpr LogLevel CH133 = CH033 | CH100; 231 | constexpr LogLevel CH200 = 8_bit; 232 | constexpr LogLevel CH201 = CH001 | CH200; 233 | constexpr LogLevel CH202 = CH002 | CH200; 234 | constexpr LogLevel CH203 = CH003 | CH200; 235 | constexpr LogLevel CH210 = CH010 | CH200; 236 | constexpr LogLevel CH211 = CH011 | CH200; 237 | constexpr LogLevel CH212 = CH012 | CH200; 238 | constexpr LogLevel CH213 = CH013 | CH200; 239 | constexpr LogLevel CH220 = CH020 | CH200; 240 | constexpr LogLevel CH221 = CH021 | CH200; 241 | constexpr LogLevel CH222 = CH022 | CH200; 242 | constexpr LogLevel CH223 = CH023 | CH200; 243 | constexpr LogLevel CH230 = CH030 | CH200; 244 | constexpr LogLevel CH231 = CH031 | CH200; 245 | constexpr LogLevel CH232 = CH032 | CH200; 246 | constexpr LogLevel CH233 = CH033 | CH200; 247 | constexpr LogLevel CH300 = 9_bit; 248 | constexpr LogLevel CH301 = CH001 | CH300; 249 | constexpr LogLevel CH302 = CH002 | CH300; 250 | constexpr LogLevel CH303 = CH003 | CH300; 251 | constexpr LogLevel CH310 = CH010 | CH300; 252 | constexpr LogLevel CH311 = CH011 | CH300; 253 | constexpr LogLevel CH312 = CH012 | CH300; 254 | constexpr LogLevel CH313 = CH013 | CH300; 255 | constexpr LogLevel CH320 = CH020 | CH300; 256 | constexpr LogLevel CH321 = CH021 | CH300; 257 | constexpr LogLevel CH322 = CH022 | CH300; 258 | constexpr LogLevel CH323 = CH023 | CH300; 259 | constexpr LogLevel CH330 = CH030 | CH300; 260 | constexpr LogLevel CH331 = CH031 | CH300; 261 | constexpr LogLevel CH332 = CH032 | CH300; 262 | constexpr LogLevel CH333 = CH033 | CH300; 263 | 264 | struct log_options 265 | { 266 | struct SCRN_t 267 | { 268 | bool on = true; 269 | } SCRN; 270 | 271 | struct SOCK_t 272 | { 273 | bool on = false; 274 | std::string ip = "127.0.0.1"; 275 | uint16_t port = 9998; 276 | } SOCK; 277 | 278 | struct FILE_t 279 | { 280 | bool on = false; 281 | bool compressed = false; 282 | uint32_t max_size_mb = 100; 283 | uint32_t max_size_all = 100 * 12; 284 | std::string directory = ""; 285 | std::string rootname = "default"; 286 | } FILE; 287 | 288 | LogLevel LVL = CH111; 289 | }; 290 | 291 | class LogLine 292 | { 293 | private: 294 | LogLine(); 295 | 296 | friend void details::psuedo_log_flush(); 297 | 298 | public: 299 | struct string_literal_t 300 | { 301 | explicit string_literal_t(const char *s) : m_s(s) {} 302 | char const *m_s; 303 | }; 304 | 305 | LogLine(LogLevel level, char const *file, char const *function, uint32_t line); 306 | 307 | LogLine(LogLevel level, char const *file, char const *function, uint32_t line, char const *ctx); 308 | 309 | void stringify(std::ostream &os, LogLevel mask = LogLevel::all(), unsigned rsh = 0); 310 | 311 | LogLevel lvl() const; 312 | 313 | LogLine &operator<<(char arg); 314 | LogLine &operator<<(int32_t arg); 315 | LogLine &operator<<(uint32_t arg); 316 | LogLine &operator<<(int64_t arg); 317 | LogLine &operator<<(uint64_t arg); 318 | LogLine &operator<<(double arg); 319 | LogLine &operator<<(const std::string &arg); 320 | 321 | template 322 | LogLine &operator<<(const char (&arg)[N]) 323 | { 324 | encode_c_string(arg, (std::max)(N, size_t(1)) - 1); 325 | return *this; 326 | } 327 | 328 | template 329 | typename std::enable_if::value, LogLine &>::type 330 | operator<<(Arg const &arg) 331 | { 332 | encode(arg); 333 | return *this; 334 | } 335 | 336 | template 337 | typename std::enable_if::value, LogLine &>::type 338 | operator<<(const Arg &arg) 339 | { 340 | encode(arg); 341 | return *this; 342 | } 343 | 344 | template 345 | typename std::enable_if() && 346 | !std::is_same::value && 347 | !std::is_same::value, 348 | LogLine &>::type 349 | operator<<(const Arg &arg) 350 | { 351 | std::ostringstream ss; 352 | ss << arg; 353 | *this << ss.str(); 354 | return *this; 355 | } 356 | 357 | private: 358 | char *buffer(); 359 | 360 | template 361 | void encode(Arg arg) 362 | { 363 | *reinterpret_cast(buffer()) = arg; 364 | m_bytes_used += sizeof(Arg); 365 | } 366 | template 367 | void encode(Arg arg, uint8_t type_id) 368 | { 369 | resize_buffer_if_needed(sizeof(Arg) + sizeof(uint8_t)); 370 | encode(type_id); 371 | encode(arg); 372 | } 373 | 374 | void encode(char *arg); 375 | void encode(char const *arg); 376 | void encode(string_literal_t arg); 377 | void encode_c_string(char const *arg, size_t length); 378 | void resize_buffer_if_needed(size_t additional_bytes); 379 | void stringify(std::ostream &os, char *start, char const *end); 380 | 381 | public: 382 | bool m_ctrl_bytes; 383 | size_t m_bytes_used; 384 | size_t m_buffer_size; 385 | std::unique_ptr m_heap_buffer; 386 | char m_stack_buffer[256 - 2 * sizeof(size_t) - sizeof(decltype(m_heap_buffer)) - 16]{}; 387 | }; 388 | 389 | namespace details 390 | { 391 | struct PsuedoLog 392 | { 393 | bool operator==(LogLine &); 394 | }; 395 | 396 | PLOG_PRINTF_CHECK(5, 6) 397 | void psuedo_log_fmt(LogLevel level, char const *file, char const *function, 398 | uint32_t line, PLOG_PRINT_STRING_TYPE format, ...); 399 | } 400 | 401 | bool is_logged(LogLevel level); 402 | 403 | void initialize_log(const log_options &opts = log_options{}); 404 | } 405 | 406 | #define PPX_LOG(LEVEL) ppx::details::PsuedoLog() == ppx::LogLine(LEVEL, ppx::filename(__FILE__), ppx::funcname(__FUNCTION__), __LINE__) 407 | #define LOG_CH(NUM) ppx::is_logged(ppx::CH##NUM) && PPX_LOG(ppx::CH##NUM) 408 | #define LOG_FMT(NUM, ...) ppx::is_logged(ppx::CH##NUM) \ 409 | ? ppx::details::psuedo_log_fmt(ppx::CH##NUM, ppx::filename(__FILE__), ppx::funcname(__FUNCTION__), __LINE__, __VA_ARGS__) \ 410 | : (void)0 411 | #define LOG_IF(NUM, COND, ...) ppx::is_logged(ppx::CH##NUM) && (cond) \ 412 | ? ppx::details::psuedo_log_fmt(ppx::CH##NUM, ppx::filename(__FILE__), ppx::funcname(__FUNCTION__), __LINE__, __VA_ARGS__) \ 413 | : (void)0 414 | #define FLUSH_LOG ppx::details::psuedo_log_flush() 415 | #endif -------------------------------------------------------------------------------- /log_server.py: -------------------------------------------------------------------------------- 1 | import socketserver 2 | 3 | 4 | class MyTCPHandler(socketserver.StreamRequestHandler): 5 | def handle(self): 6 | # self.rfile is a file-like object created by the handler; 7 | # we can now use e.g. readline() instead of raw recv() calls 8 | len_data = 1 9 | while len_data != 0: 10 | data = self.rfile.readline().strip() 11 | len_data = len(data) 12 | print(data.decode()) 13 | 14 | 15 | if __name__ == "__main__": 16 | HOST, PORT = "localhost", 9999 17 | 18 | # Create the server, binding to localhost on port 9999 19 | with socketserver.TCPServer((HOST, PORT), MyTCPHandler) as server: 20 | # Activate the server; this will keep running until you 21 | # interrupt the program with Ctrl-C 22 | server.serve_forever() 23 | -------------------------------------------------------------------------------- /show_trans.py: -------------------------------------------------------------------------------- 1 | """Simulated UR6 Robotics""" 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import matplotlib.animation as animation 5 | from pytransform3d.plot_utils import make_3d_axis 6 | import modern_robotics as mr 7 | from tools.misc import VisualPose 8 | from tools.misc import VisualLink 9 | 10 | 11 | if __name__ == "__main__": 12 | plt.style.use("tools.gnuplot") 13 | fig = plt.figure() 14 | ax = make_3d_axis(ax_s=2) 15 | 16 | NFRAME = 200 17 | trans_strat = mr.MatrixExp6(mr.VecTose3([0, 0, 0, 0, 0, 1])) 18 | trans_end = mr.MatrixExp6(mr.VecTose3([np.pi / 2, np.pi / 2, np.pi / 2, 0, 0, 0])) 19 | 20 | transform_start = VisualPose(ax, trans_strat, traj_len=0, scale=0.5) 21 | transform_end = VisualPose(ax, trans_end, traj_len=0, scale=0.5) 22 | obj = VisualPose(ax, trans_strat, traj_len=50, scale=0.5) 23 | 24 | def update_obj(cur_f): 25 | """Collect all animated objs to update.""" 26 | screw = mr.MatrixLog6(mr.TransInv(trans_strat) @ trans_end) 27 | vec_screw = mr.se3ToVec(screw) 28 | vec_screw[1] = 0.0 29 | screw = mr.VecTose3(vec_screw) 30 | tend = mr.MatrixExp6(cur_f / NFRAME * screw) 31 | obj.update(trans_strat @ tend) 32 | 33 | def init_obj(): 34 | """Collect all animated objs to init.""" 35 | obj.clear() 36 | obj.update(trans_strat) 37 | 38 | anim = animation.FuncAnimation( 39 | fig, 40 | update_obj, 41 | NFRAME, 42 | init_obj, 43 | interval=25, 44 | ) 45 | 46 | plt.show() 47 | -------------------------------------------------------------------------------- /signals.py: -------------------------------------------------------------------------------- 1 | import array 2 | import numpy as np 3 | from scipy import signal 4 | import matplotlib.pyplot as plt 5 | import tools.misc as misc 6 | 7 | plt.style.use("tools.gnuplot") 8 | plt.axis("equal") 9 | 10 | 11 | def Hu(u): 12 | nu = u / np.linalg.norm(u) 13 | return np.eye(2) - 2 * nu @ np.transpose(nu) 14 | 15 | 16 | u = np.array([[3, 1]]).T 17 | line1 = np.array([[0, 0], [0.25, 1], [0.5, 2], [0.75, 3], [1, 4]]) 18 | line2 = np.array([Hu(u) @ np.array(ele) for ele in line1]) 19 | plt.plot(line2[:, 0], line2[:, 1], "x-") 20 | plt.plot(line1[:, 0], line1[:, 1], "x-") 21 | plt.plot([0, 3], [0, 1], label=r"$u$") 22 | plt.plot([-1, 1], [3, -3], label=r"$u_\bot$") 23 | plt.legend() 24 | 25 | # N = 2048 * 5 # 采样点的个数 26 | # x = np.arange(0, 2 * np.pi, 2 * np.pi / N) 27 | # fs = N / (2 * np.pi) 28 | # # 产生频率为120、500、10hz的信号进行模拟 29 | # y = ( 30 | # 7 * np.sin(120 * 2 * np.pi * x) 31 | # + 5 * np.sin(500 * 2 * np.pi * x) 32 | # + 9 * np.sin(10 * 2 * np.pi * x) 33 | # + np.random.normal(scale=np.sqrt(1.89), size=len(x)) 34 | # ) 35 | # fre, amp, _ = misc.fft(y, fs) 36 | # plt.plot(fre, amp) 37 | # w = np.arange(0, N, 1) # 频域轴 38 | 39 | 40 | # b1 = signal.firwin( 41 | # 51, [0.42, 0.8], window="hamming", pass_zero="bandstop" 42 | # ) # 哈明窗，截至频率100Hz 43 | # b2 = [ 44 | # 0.0010176, 45 | # 0.000927711, 46 | # -0.00238796, 47 | # 0.000434621, 48 | # -0.00010929, 49 | # 0.00253823, 50 | # 0.00180104, 51 | # -0.00843451, 52 | # 0.00289083, 53 | # 0.00124462, 54 | # 0.00682364, 55 | # 0.00227208, 56 | # -0.0243103, 57 | # 0.0124667, 58 | # 0.00617722, 59 | # 0.0127504, 60 | # -0.00127084, 61 | # -0.0580584, 62 | # 0.0431386, 63 | # 0.0189968, 64 | # 0.0179269, 65 | # -0.0191777, 66 | # -0.172457, 67 | # 0.224718, 68 | # 0.120414, 69 | # 0.619334, 70 | # 0.120414, 71 | # 0.224718, 72 | # -0.172457, 73 | # -0.0191777, 74 | # 0.0179269, 75 | # 0.0189968, 76 | # 0.0431386, 77 | # -0.0580584, 78 | # -0.00127084, 79 | # 0.0127504, 80 | # 0.00617722, 81 | # 0.0124667, 82 | # -0.0243103, 83 | # 0.00227208, 84 | # 0.00682364, 85 | # 0.00124462, 86 | # 0.00289083, 87 | # -0.00843451, 88 | # 0.00180104, 89 | # 0.00253823, 90 | # -0.00010929, 91 | # 0.000434621, 92 | # -0.00238796, 93 | # 0.000927711, 94 | # 0.0010176, 95 | # ] 96 | # w1, h = signal.freqz(b1) # 求频响 97 | # w2, h2 = signal.freqz(b2) 98 | 99 | # plt.figure(1) 100 | # plt.title("Frequence Response") 101 | # plt.plot(w1 / 2 / np.pi * N, 20 * np.log10(np.abs(h) + 0.01)) 102 | # plt.xlabel("$f$") 103 | # plt.ylabel("$H(f)$") 104 | # plt.plot(w2 / 2 / np.pi * N, 20 * np.log10(np.abs(h2) + 0.01)) 105 | 106 | # b2 = signal.firwin(24, 2 * 100 / N, window="hann") # 汉宁窗，截至频率100Hz 107 | # w1, h = signal.freqz(b2) # 求频响 108 | # plt.figure(2) 109 | # plt.title("freqz") 110 | # plt.plot(w1 / 2 / np.pi * N, 20 * np.log10(np.abs(h) + 0.01)) 111 | 112 | # b3 = signal.firwin(24, 2 * 100 / N, window="blackman") # 布莱克曼窗，截至频率100Hz 113 | # w1, h = signal.freqz(b3) # 求频响 114 | # plt.figure(3) 115 | # plt.title("freqz") 116 | # plt.plot(w1 / 2 / np.pi * N, 20 * np.log10(np.abs(h) + 0.01)) 117 | 118 | # b4 = signal.firwin(N, 2 * 100 / N, window="boxcar") # 矩形窗，截至频率100Hz 119 | # w1, h = signal.freqz(b4) # 求频响 120 | # plt.figure(4) 121 | # plt.title("freqz") 122 | # plt.plot(w1 / 2 / np.pi * N, 20 * np.log10(np.abs(h) + 0.01)) 123 | plt.show() 124 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(SUBPRJ "test") 2 | message(STATUS "---------------------------") 3 | message(STATUS "Current : ${SUBPRJ}") 4 | if (ENABLE_UNIT_TESTS) 5 | message(STATUS "Enable Gtest in ${SUBPRJ}") 6 | message(STATUS "-- Gtest Configure") 7 | include(FetchContent) 8 | FetchContent_Declare( 9 | googletest 10 | GIT_REPOSITORY https://github.com/google/googletest.git 11 | GIT_TAG v1.14.0 12 | ) 13 | if (CMAKE_CXX_COMPILER_ID MATCHES MSVC) 14 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 15 | endif () 16 | FetchContent_MakeAvailable(googletest) 17 | include(GoogleTest) 18 | add_executable(matrix_test matrix_test.cpp) 19 | target_link_libraries(matrix_test PRIVATE ppx gtest_main) 20 | add_executable(lineqn_test solver_test.cpp) 21 | target_link_libraries(lineqn_test PRIVATE ppx gtest_main) 22 | add_executable(opt_test opt_test.cpp) 23 | target_link_libraries(opt_test PRIVATE ppx gtest_main) 24 | add_executable(lie_test lie_test.cpp) 25 | target_link_libraries(lie_test PRIVATE ppx gtest_main) 26 | add_executable(signals_test signals_test.cpp) 27 | target_link_libraries(signals_test PRIVATE ppx gtest_main) 28 | add_executable(statistic_test statistic_test.cpp) 29 | target_link_libraries(statistic_test PRIVATE ppx gtest_main) 30 | add_executable(log_test log_test.cpp) 31 | target_link_libraries(log_test PRIVATE plog gtest_main) 32 | gtest_discover_tests(matrix_test 33 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/build/test) 34 | gtest_discover_tests(lineqn_test 35 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/build/test) 36 | gtest_discover_tests(opt_test 37 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/build/test) 38 | gtest_discover_tests(lie_test 39 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/build/test) 40 | gtest_discover_tests(signals_test 41 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/build/test) 42 | gtest_discover_tests(statistic_test 43 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/build/test) 44 | gtest_discover_tests(log_test 45 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/build/test) 46 | if (ENABLE_LCOV) 47 | message(STATUS "Enable Lcov in ${SUBPRJ}") 48 | message(STATUS "-- lcov Configure") 49 | find_program(LCOV_BIN lcov) 50 | if (NOT LCOV_BIN MATCHES "lcov$") 51 | message(FATAL_ERROR "lcov required, but not found!") 52 | endif () 53 | if (CMAKE_BUILD_TYPE MATCHES Debug) 54 | include(CodeCoverage.cmake) 55 | APPEND_COVERAGE_COMPILER_FLAGS() 56 | setup_target_for_coverage_lcov( 57 | NAME coverage 58 | EXECUTABLE ctest test 59 | BASE_DIRECTORY "${PROJECT_SOURCE_DIR}/build" 60 | EXCLUDE "${PROJECT_SOURCE_DIR}/build/_deps/*" "/usr/*" 61 | DEPENDENCIES matrix_test lineqn_test opt_test lie_test statistic_test signals_test log_test) 62 | endif () 63 | endif () 64 | endif () -------------------------------------------------------------------------------- /test/lie_test.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include "liegroup.hpp" 3 | 4 | using namespace ppx; 5 | 6 | struct joint 7 | { 8 | std::string name{"Anon"}; 9 | se3 screw; 10 | SE3 pose; 11 | joint() = default; 12 | joint(const std::string &name_, const se3 &screw_, const SE3 &pose_) 13 | : name(name_), screw(screw_), pose(pose_){}; 14 | }; 15 | 16 | template 17 | class kinematics 18 | { 19 | private: 20 | template 21 | using idx_available_t = typename std::enable_if<(L < N), RT>::type; 22 | std::array JList; 23 | 24 | public: 25 | using Q = MatrixS; 26 | template 27 | idx_available_t Joint() 28 | { 29 | return JList[L]; 30 | } 31 | template 32 | idx_available_t Joint() const 33 | { 34 | return JList[L]; 35 | } 36 | template 37 | idx_available_t setJoint(const joint &j) 38 | { 39 | JList[L] = j; 40 | } 41 | SE3 forwardSpace(const std::string &jointName, const Q &jointAngle) const 42 | { 43 | auto effector_idx = std::find_if(JList.begin(), JList.end(), [&jointName](const joint &elem) 44 | { return jointName == elem.name; }); 45 | if (effector_idx == JList.end()) 46 | { 47 | return {}; 48 | } 49 | SE3 effector_pose = effector_idx->pose; 50 | for (int i = (int)std::distance(JList.begin(), effector_idx); i > -1; i--) 51 | { 52 | se3 screw = JList[i].screw * jointAngle[i]; 53 | effector_pose = screw.exp() * effector_pose; 54 | } 55 | return effector_pose; 56 | } 57 | SE3 forwardSpace(const Q &jointAngle) const 58 | { 59 | SE3 effector_pose = JList.back().pose; 60 | for (int i = N - 1; i > -1; i--) 61 | { 62 | se3 screw = JList[i].screw * jointAngle[i]; 63 | effector_pose = screw.exp() * effector_pose; 64 | } 65 | return effector_pose; 66 | } 67 | MatrixS<6, N> jacobiSpace(const Q &jointAngle) 68 | { 69 | MatrixS<6, N> Js; 70 | SE3 T; 71 | for (int i = 1; i < (int)N; i++) 72 | { 73 | se3 screw = JList[i - 1].screw * jointAngle[i - 1]; 74 | T = T * screw.exp(); 75 | Js.template sub<6, 1>(0, i) = T.Adt() * JList[i].screw; 76 | } 77 | return Js; 78 | } 79 | template 80 | idx_available_t> jacobiSpace(const std::array &namelist, const Q &jointAngle) 81 | { 82 | MatrixS<6, L> Js; 83 | auto JsAll = jacobiSpace(jointAngle); 84 | for (size_t i = 0; i < L; i++) 85 | { 86 | auto iter = std::find_if(JList.begin(), JList.end(), [&namelist, i](const joint &elem) 87 | { return namelist[i] == elem.name; }); 88 | if (iter != JList.end()) 89 | { 90 | auto col_idx = std::distance(JList.begin(), iter); 91 | Js.template sub<6, 1>(0, i) = JsAll.template sub<6, 1>(0, col_idx); 92 | } 93 | } 94 | return Js; 95 | } 96 | Q inverseSpace(const SE3 &pose, Q init) 97 | { 98 | SE3 Tsb; 99 | se3 Vs; 100 | bool converage = false; 101 | auto iter = 0u; 102 | while (!converage && iter < 20) 103 | { 104 | Tsb = forwardSpace(init); 105 | Vs = Tsb.Adt() * (Tsb.I() * pose).log(); 106 | auto err_w = norm2(Vs._1()); 107 | auto err_v = norm2(Vs._2()); 108 | printf("iter=%d, w_error=%f, v_error=%f\n", iter, err_w, err_v); 109 | converage = err_w < EPS_SP && err_v < EPS_SP; 110 | init += linsolve(jacobiSpace(init), Vs).x; 111 | ++iter; 112 | } 113 | return init; 114 | // auto fn = [this, pose](const Q &x) 115 | // { 116 | // auto Tsb = forwardSpace(x); 117 | // auto Vs = Tsb.Adt() * (Tsb.I() * pose).log(); 118 | // return 0.5 * inner_product(Vs, Vs); 119 | // }; 120 | // auto dfn = [this, pose](const Q &x) 121 | // { 122 | // auto Tsb = forwardSpace(x); 123 | // se3 Vs = Tsb.Adt() * (Tsb.I() * pose).log(); 124 | // return Q(jacobiSpace(x) * Vs); 125 | // }; 126 | // return fminunc(fn, dfn, init).x; 127 | } 128 | }; 129 | 130 | class LieGroup_TestCase : public ::testing::Test 131 | { 132 | public: 133 | LieGroup_TestCase() = default; 134 | }; 135 | 136 | TEST_F(LieGroup_TestCase, so3) 137 | { 138 | MatrixS<3, 1> vec{1, 2, 3}; 139 | SO3 expected{0, 3, -2, -3, 0, 1, 2, -1, 0}; 140 | SO3 result = hat(vec); 141 | EXPECT_EQ(result, expected); 142 | } 143 | 144 | TEST_F(LieGroup_TestCase, SO3) 145 | { 146 | for (int i = -179; i < 180; i++) 147 | { 148 | so3 w{0.0, 0.0, DEG_RAD((double)i)}; 149 | auto R = w.exp(); 150 | EXPECT_EQ(SO3::RotZ(DEG_RAD((double)i)), R); 151 | auto dR = R.log(); 152 | EXPECT_NEAR(norm2((dR - w).eval()), 0.0, EPS_SP); 153 | } 154 | } 155 | 156 | TEST_F(LieGroup_TestCase, SE3) 157 | { 158 | SE3 Tinput{1, 0, 0, 0, 159 | 0, 0, 1, 0, 160 | 0, -1, 0, 0, 161 | 0, 0, 3, 1}; 162 | MatrixS<4, 4> expected{0.0, 0.0, 0.0, 0.0, 163 | 0.0, 0.0, 1.57079633, 0.0, 164 | 0.0, -1.57079633, 0.0, 0.0, 165 | 0.0, 2.35619449, 2.35619449, 0.0}; 166 | auto result = hat(Tinput.log()); 167 | EXPECT_EQ(result, expected); 168 | } 169 | 170 | TEST_F(LieGroup_TestCase, se3) 171 | { 172 | MatrixS<4, 4> se3mat = {0.0, 0.0, 0.0, 0.0, 173 | 0.0, 0.0, PI / 2.0, 0.0, 174 | 0.0, -PI / 2.0, 0.0, 0.0, 175 | 0.0, 2.3562, 2.3562, 0.0}; 176 | SE3 result{{1, 0, 0, 0, 0, 1, 0, -1, 0, 0}, {0, 0, 3}}; 177 | auto cal = vee(se3mat).exp(); 178 | for (size_t i = 0; i < 16; i++) 179 | { 180 | EXPECT_NEAR(cal[i], result[i], 1.0e-5); 181 | } 182 | } 183 | 184 | TEST_F(LieGroup_TestCase, kinematics) 185 | { 186 | kinematics<6> UR5; 187 | SE3 F6{-1.0, 0.0, 0.0, 0.0, 188 | 0.0, 0.0, 1.0, 0.0, 189 | 0.0, 1.0, 0.0, 0.0, 190 | 0.817, 0.191, -0.006, 1.0}; 191 | UR5.setJoint<0>({"R1", se3{0, 0, 1, 0, 0, 0, 0}, SE3()}); 192 | UR5.setJoint<1>({"R2", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.0}, SE3{}}); 193 | UR5.setJoint<2>({"R3", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.425}, SE3{}}); 194 | UR5.setJoint<3>({"R4", se3{0.0, 1.0, 0.0, -0.089, 0.0, 0.817}, SE3{}}); 195 | UR5.setJoint<4>({"R5", se3{0.0, 0.0, -1.0, -0.109, 0.817, 0.0}, SE3{}}); 196 | UR5.setJoint<5>({"R6", se3{0.0, 1.0, 0.0, 0.006, 0.0, 0.817}, F6}); 197 | SE3 result{0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.095, 0.109, 0.988, 1.0}; 198 | auto r = UR5.forwardSpace("R6", {0.0, -0.5 * PI, 0.0, 0.0, 0.5 * PI, 0.0}); 199 | EXPECT_EQ(r, result); 200 | MatrixS<6, 6> result2{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 201 | 0.0, 1.0, 0.0, -0.089, 0.0, 0.0, 202 | 0.0, 1.0, 0.0, -0.514, 0.0, 0.0, 203 | 0.0, 1.0, 0.0, -0.906, 0.0, 0.0, 204 | 1.0, 0.0, 0.0, 0.0, 0.906, -0.109, 205 | 0.0, 0.0, 1.0, 0.109, -0.095, 0.0}; 206 | auto r2 = UR5.jacobiSpace({0.0, -0.5 * PI, 0.0, 0.0, 0.5 * PI, 0.0}); 207 | EXPECT_EQ(r2, result2); 208 | auto r3 = UR5.jacobiSpace(std::array{"R1", "R2", "R3"}, {0.0, -0.5 * PI, 0.0, 0.0, 0.5 * PI, 0.0}); 209 | MatrixS<6, 3> result3 = r2.sub<6, 3>(0, 0); 210 | EXPECT_EQ(r3, result3); 211 | SE3 TargetPose{0.0, 1.0, 0.0, 0.0, 212 | -1.0, 0.0, 0.0, 0.0, 213 | 0.0, 0.0, 1.0, 0.0, 214 | 0.095, 0.109, 0.988, 1.0}; 215 | auto j = UR5.inverseSpace(TargetPose, {0.0, -1.5, 0.0, 0.0, 1.5, 0.0}); 216 | EXPECT_EQ(UR5.forwardSpace("R6", j), TargetPose); 217 | } -------------------------------------------------------------------------------- /test/log_test.cpp: -------------------------------------------------------------------------------- 1 | #include "plog.h" 2 | #include "gtest/gtest.h" 3 | #include 4 | #include 5 | 6 | #define LOGD(X) LOG_CH(111) << X 7 | 8 | class PLOG_TestCase : public ::testing::Test 9 | { 10 | public: 11 | PLOG_TestCase() = default; 12 | 13 | static void SetUpTestSuite() 14 | { 15 | ppx::log_options opts; 16 | opts.FILE.on = true; 17 | opts.FILE.max_size_mb = 10; 18 | opts.FILE.max_size_all = 50; 19 | ppx::initialize_log(opts); 20 | } 21 | }; 22 | 23 | TEST_F(PLOG_TestCase, quick_log) 24 | { 25 | constexpr int argv = 3; 26 | constexpr char test_data[56] = "1234567890abcdefghigklmnopqrstuvwxyz_zyw_zsummer_log4z"; 27 | LOGD("char:" << 'c' 28 | << ", unsigned char:" << (unsigned char)'c' 29 | << ", short:" << (short)-1 30 | << ", unsigned short:" << (unsigned short)-1 31 | << ", int:" << (int)-1 32 | << ", unsigned int:" << (unsigned int)-1 33 | << ", long:" << (long)-1 34 | << ", unsigned long:" << (unsigned long)-1 35 | << ", long long:" << (long long)-1 36 | << ", unsigned long long:" << (unsigned long long)-1 37 | << ", float:" << (float)-1.234567 38 | << ", double:" << (double)-2.34566 39 | << ", double:" << pow(2, 52) - 1.0 40 | << ", double:" << pow(2, 52) * -1000 41 | << ", double:" << pow(2, 52) / 1000 42 | << ", double:" << pow(2, 52) / -1000 43 | << ", double:" << pow(2, -58) 44 | << ", double:" << pow(2, -16) * -1 45 | << ", std::string:" << std::string("fffff") 46 | << ", int *:" << (int *)argv 47 | << ", const int *:" << (const int *)argv 48 | << ", constant:" << 1000 49 | << ", constant:" << 100.12345678 50 | << ", bool:" << true 51 | << ", show const char* data:" << test_data); 52 | } 53 | 54 | TEST_F(PLOG_TestCase, rand_log) 55 | { 56 | for (size_t j = 0; j < 1000; j++) 57 | { 58 | for (size_t i = 0; i < 100; i++) 59 | { 60 | int r = rand() % 14; 61 | switch (r) 62 | { 63 | case 0: 64 | LOG_CH(111) << 'c'; 65 | break; 66 | case 1: 67 | LOG_CH(111) << UINT8_MAX; 68 | break; 69 | case 2: 70 | LOG_CH(111) << INT16_MIN; 71 | break; 72 | case 3: 73 | LOG_CH(111) << INT16_MAX; 74 | break; 75 | case 4: 76 | LOG_CH(111) << UINT16_MAX; 77 | break; 78 | case 5: 79 | LOG_CH(111) << INT32_MIN; 80 | break; 81 | case 6: 82 | LOG_CH(111) << INT32_MAX; 83 | break; 84 | case 7: 85 | LOG_CH(111) << UINT32_MAX; 86 | break; 87 | case 8: 88 | LOG_CH(111) << INT64_MIN; 89 | break; 90 | case 9: 91 | LOG_CH(111) << INT64_MAX; 92 | break; 93 | case 10: 94 | LOG_CH(111) << UINT64_MAX; 95 | break; 96 | case 11: 97 | LOG_CH(111) << (float)pow(rand() % 100, rand() % 20) * ((rand() % 2 == 0 ? -1.0 : 1.0)); 98 | break; 99 | case 12: 100 | LOG_CH(111) << (double)pow(rand() % 100, rand() % 200) * ((rand() % 2 == 0 ? -1.0 : 1.0)); 101 | break; 102 | default: 103 | LOG_CH(111) << "8"; 104 | break; 105 | } 106 | } 107 | } 108 | } -------------------------------------------------------------------------------- /test/opt_test.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include "liegroup.hpp" 3 | 4 | using namespace ppx; 5 | 6 | class NonLinOpt_TestCase : public ::testing::Test 7 | { 8 | public: 9 | NonLinOpt_TestCase() = default; 10 | }; 11 | 12 | TEST_F(NonLinOpt_TestCase, OneDimension_OPT) 13 | { 14 | auto f1 = [](double x) 15 | { 16 | double y = 0.0; 17 | for (int k = -10; k < 11; k++) 18 | { 19 | y += (k + 1) * (k + 1) * cos(k * x) * exp(-1 * k * k / 2.0); 20 | } 21 | return y; 22 | }; 23 | 24 | auto x1 = fminbnd(f1, 1.0, 3.0); 25 | auto x2 = fminbnd(f1, 1.0, 3.0); 26 | auto x3 = fminbnd(f1, 1.0, 3.0); 27 | EXPECT_NEAR(x1.x, 2.006062180123711, ppx::EPS_SP * 1e2); 28 | EXPECT_NEAR(x2.x, 2.006062180123711, ppx::EPS_SP * 1e2); 29 | EXPECT_NEAR(x3.x, 2.006062180123711, ppx::EPS_SP * 1e2); 30 | 31 | auto f2 = [](double x) 32 | { 33 | return sin(x); 34 | }; 35 | 36 | x1 = fminbnd(f2, 0.0, 2.0 * PI); 37 | x2 = fminbnd(f2, 0.0, 2.0 * PI); 38 | x3 = fminbnd(f2, 0.0, 2.0 * PI); 39 | 40 | EXPECT_NEAR(x1.x, 4.712391081200089, ppx::EPS_SP * 1e2); 41 | EXPECT_NEAR(x2.x, 4.712391081200089, ppx::EPS_SP * 1e2); 42 | EXPECT_NEAR(x3.x, 4.712391081200089, ppx::EPS_SP * 1e2); 43 | 44 | auto f3 = [](double x) 45 | { 46 | return sin(x - 9.0 / 7.0); 47 | }; 48 | 49 | x1 = fminbnd(f3, 1, 2.0 * PI); 50 | x2 = fminbnd(f3, 1, 2.0 * PI); 51 | x3 = fminbnd(f3, 1, 2.0 * PI); 52 | 53 | EXPECT_NEAR(x1.x, 5.998103062276137, ppx::EPS_SP * 1e2); 54 | EXPECT_NEAR(x3.x, 5.998103062276137, ppx::EPS_SP * 1e2); 55 | } 56 | 57 | TEST_F(NonLinOpt_TestCase, MultiDimension_OPT) 58 | { 59 | auto f4 = [](const MatrixS<2, 1> &x) 60 | { 61 | return 3 * x[0] * x[0] + 2 * x[0] * x[1] + x[1] * x[1] - 4 * x[0] + 5 * x[1]; 62 | }; 63 | 64 | MatrixS<2, 1> result{2.250000572016352, -4.749999832427862}; 65 | auto x1 = fminunc(f4, MatrixS<2, 1>{1, 1}); 66 | EXPECT_LE(norminf((x1.x - result).eval()), 1.0e-3); 67 | 68 | auto f5 = [](const MatrixS<2, 1> &x) 69 | { 70 | auto sqr = x[0] * x[0] + x[1] * x[1]; 71 | return x[0] * exp(-sqr) + sqr / 20.0; 72 | }; 73 | result = {-0.669071393870035, 7.095255048137055e-07}; 74 | x1 = fminunc(f5, MatrixS<2, 1>{1, 2}); 75 | EXPECT_LE(norminf((x1.x - result).eval()), 1.0e-3); 76 | 77 | auto f6 = [](const MatrixS<2, 1> &x) 78 | { 79 | return 100 * (x[1] - x[0] * x[0]) * (x[1] - x[0] * x[0]) + (1 - x[0]) * (1 - x[0]); 80 | }; 81 | 82 | auto d6 = [](const MatrixS<2, 1> &x) 83 | { 84 | MatrixS<2, 1> y; 85 | y[0] = -400 * (x[1] - x[0] * x[0]) * x[0] - 2 * (1 - x[0]); 86 | y[1] = 200 * (x[1] - x[0] * x[0]); 87 | return y; 88 | }; 89 | 90 | x1 = fminunc(f6, MatrixS<2, 1>{4, -8}); 91 | auto x2 = fminunc(f6, d6, MatrixS<2, 1>{4, -8}); 92 | auto x3 = fminunc(f6, d6, MatrixS<2, 1>{4, -8}); 93 | auto x4 = fminunc(f6, d6, MatrixS<2, 1>{4, -8}); 94 | result = {1, 1}; 95 | EXPECT_EQ(x1.x, result); 96 | // EXPECT_EQ(x2.x, result); 97 | EXPECT_EQ(x3.x, result); 98 | EXPECT_EQ(x4.x, result); 99 | 100 | auto f7 = [](const MatrixS<2, 1> &x) 101 | { 102 | return (x[0] + 2 * x[1] - 7) * (x[0] + 2 * x[1] - 7) + (2 * x[0] + x[1] - 5) * (2 * x[0] + x[1] - 5); 103 | }; 104 | 105 | auto d7 = [](const MatrixS<2, 1> &x) 106 | { 107 | MatrixS<2, 1> y; 108 | y[0] = 10 * x[0] + 8 * x[1] - 34; 109 | y[1] = 8 * x[0] + 10 * x[1] - 38; 110 | return y; 111 | }; 112 | 113 | x1 = fminunc(f7, MatrixS<2, 1>{9, 8}); 114 | x2 = fminunc(f7, d7, MatrixS<2, 1>{9, 8}); 115 | x3 = fminunc(f7, d7, MatrixS<2, 1>{9, 8}); 116 | x4 = fminunc(f7, d7, MatrixS<2, 1>{9, 8}); 117 | EXPECT_EQ(x1.x, x2.x); 118 | EXPECT_EQ(x2.x, x3.x); 119 | EXPECT_EQ(x3.x, x4.x); 120 | } 121 | 122 | TEST_F(NonLinOpt_TestCase, CodoTest) 123 | { 124 | auto chem11 = [](const MatrixS<11, 1> &n) 125 | { 126 | // Chemical equilibrium system 127 | constexpr auto p = 40; 128 | constexpr auto R = 10; 129 | constexpr auto K5 = 1.930e-1; 130 | constexpr auto K6 = 2.597e-3; 131 | constexpr auto K7 = 3.448e-3; 132 | constexpr auto K8 = 1.799e-5; 133 | constexpr auto K9 = 2.155e-4; 134 | constexpr auto K10 = 3.846e-5; 135 | 136 | MatrixS<11, 1> f; 137 | auto rap = p / n[10]; 138 | f[0] = n[0] + n[3] - 3.; 139 | f[1] = 2 * n[0] + n[1] + n[3] + n[6] + n[7] + n[8] + 2 * n[9] - R; 140 | f[2] = 2 * n[1] + 2 * n[4] + n[5] + n[6] - 8; 141 | f[3] = 2 * n[2] + n[8] - 4 * R; 142 | f[4] = K5 * n[1] * n[3] - n[0] * n[4]; 143 | f[5] = K6 * K6 * n[1] * n[3] - n[0] * n[5] * n[5] * rap; 144 | f[6] = K7 * K7 * n[0] * n[1] - n[3] * n[6] * n[6] * rap; 145 | f[7] = K8 * n[0] - n[3] * n[7] * rap; 146 | f[8] = K9 * K9 * n[0] * n[0] * n[2] - n[3] * n[3] * n[8] * n[8] * rap; 147 | f[9] = K10 * n[0] * n[0] - n[3] * n[3] * n[9] * rap; 148 | f[10] = n[10] - std::accumulate(n.data(), n.data() + 10, 0.0); 149 | return f; 150 | }; 151 | 152 | MatrixS<11, 1> init_x, lower, upper; 153 | init_x.fill(1); 154 | lower.fill(0); 155 | upper.fill(MAX_SP); 156 | details::CoDo<11, 11> codo(chem11, lower, upper); 157 | auto result = codo(init_x); 158 | //2.91573 3.96094 19.9863 0.0842748 0.0220957 0.000722779 0.0332022 0.000421098 0.0274166 0.0311465 27.0622 159 | EXPECT_TRUE(result.y < EPS_SP); 160 | } -------------------------------------------------------------------------------- /test/signals_test.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include "signals.hpp" 3 | 4 | using namespace ppx; 5 | 6 | bool compare_vector(const std::vector &expected, const std::vector &input, double threshold = 10e-5) 7 | { 8 | return std::equal(expected.begin(), expected.end(), input.begin(), [threshold](double a, double b) 9 | { auto diff=fabs(a - b); 10 | if(diff &input, const std::vector &expected) 19 | { 20 | std::vector results, velocity, expects_vel; 21 | for (auto i : input) 22 | { 23 | results.push_back(flt(i)); 24 | velocity.push_back(flt.diff()); 25 | } 26 | expects_vel.push_back(0.0); 27 | for (size_t i = 1; i < expected.size(); i++) 28 | { 29 | expects_vel.push_back(expected[i] - expected[i - 1]); 30 | } 31 | 32 | EXPECT_TRUE(compare_vector(expected, results)); 33 | EXPECT_TRUE(compare_vector(velocity, expects_vel)); 34 | } 35 | 36 | class SIG_TestCase : public ::testing::Test 37 | { 38 | public: 39 | SIG_TestCase() : rand() 40 | { 41 | for (int i = 0; i < 100; ++i) 42 | { 43 | double sample_point = -PI + (double)i / 50 * PI; 44 | datas1.emplace_back(sin(sample_point) + rand()[0]); 45 | } 46 | for (int i = 0; i < 8; i++) 47 | { 48 | datas2.emplace_back(i); 49 | } 50 | }; 51 | MultiGaussianDistribution<1> rand; 52 | std::vector datas1; 53 | std::vector datas2; 54 | }; 55 | 56 | TEST_F(SIG_TestCase, FIRFilter_LP) 57 | { 58 | std::vector expected{0.0679901667389975, 0.864019666522005, 0.0679901667389975}; 59 | std::vector results{0.0, 0.0680, 1.0000, 2.0000, 3.0000, 4.0000, 5.0000, 6.0000}; 60 | FIRFilter<3, FreqProperty::LowPass> flt(0.1, FIRType::Hamming, false); 61 | EXPECT_TRUE(compare_vector(expected, flt.coff_b())); 62 | test_filter(flt, datas2, results); 63 | } 64 | 65 | TEST_F(SIG_TestCase, FIRFilter_HP) 66 | { 67 | std::vector expected{6.65228083586735e-18, 0.00930428314501813, -0.0475777661344174, 0.122363546361145, -0.202246558429840, 0.237015691859159, -0.202246558429840, 0.122363546361145, -0.0475777661344174, 0.00930428314501813, 6.65228083586735e-18}; 68 | std::vector results{0, 6.65228083586735e-18, 0.00930428314501815, -0.0289691998443812, 0.0551208635273642, -0.0630356315307308, 0.0558235652703332, -0.0275637963584431}; 69 | FIRFilter<11, FreqProperty::HighPass> flt(0.8, FIRType::Hamming, false); 70 | EXPECT_TRUE(compare_vector(expected, flt.coff_b())); 71 | test_filter(flt, datas2, results); 72 | } 73 | 74 | TEST_F(SIG_TestCase, FIRFilter_BS) 75 | { 76 | std::vector expected{-0.00234328376094847, 0.00699997933273476, 0.0242756533581922, -0.0102228877948111, 0.962581077729665, -0.0102228877948111, 0.0242756533581922, 0.00699997933273476, -0.00234328376094847}; 77 | std::vector results{0, -0.00234328376094847, 0.00231341181083782, 0.0312457607408164, 0.0499552218759838, 1.03124576074082, 2.00231341181084, 2.99765671623905}; 78 | FIRFilter<9, FreqProperty::BandStop> flt(0.4, 0.45, FIRType::Hamming, false); 79 | EXPECT_TRUE(compare_vector(expected, flt.coff_b())); 80 | test_filter(flt, datas2, results); 81 | } 82 | 83 | TEST_F(SIG_TestCase, IIRFilter_LP) 84 | { 85 | std::vector expected_b{8.57655707325943e-06, 5.14593424395566e-05, 0.000128648356098892, 0.000171531141465189, 0.000128648356098892, 5.14593424395566e-05, 8.57655707325943e-06}; 86 | std::vector expected_a{1, -4.78713549885213, 9.64951772872191, -10.4690788925439, 6.44111188100806, -2.12903875003045, 0.295172431349155}; 87 | std::vector results{0, 8.57655707325942e-06, 0.000109669597409407, 0.000699540295571364, 0.00299783621920436, 0.00980009466680698, 0.0262688145029736, 0.0604915116363023}; 88 | IIRFilter<6, FreqProperty::LowPass> flt(0.1, false); 89 | EXPECT_TRUE(compare_vector(expected_a, flt.coff_a())); 90 | EXPECT_TRUE(compare_vector(expected_b, flt.coff_b())); 91 | test_filter(flt, datas2, results); 92 | } 93 | 94 | TEST_F(SIG_TestCase, IIRFilter_HP) 95 | { 96 | std::vector expected_b{0.107998403757355, -0.755988826301485, 2.26796647890446, -3.77994413150743, 3.77994413150743, -2.26796647890446, 0.755988826301485, -0.107998403757355}; 97 | std::vector expected_a{1, -2.78251380035720, 3.96680789197413, -3.40515039448060, 1.87586271598869, -0.650945698531145, 0.130852451560490, -0.0116627280491896}; 98 | std::vector results{0, 0.107998403757355, -0.239484969915386, -0.0147951165394243, 0.196590022304780, 0.127623335139356, -0.0635571388890397, -0.155957236529292}; 99 | IIRFilter<7, FreqProperty::HighPass> flt(0.3, false); 100 | EXPECT_TRUE(compare_vector(expected_a, flt.coff_a())); 101 | EXPECT_TRUE(compare_vector(expected_b, flt.coff_b())); 102 | test_filter(flt, datas2, results); 103 | } 104 | 105 | TEST_F(SIG_TestCase, IIRFilter_BS) 106 | { 107 | std::vector expected_b{0.809066862358596, -6.22565762049704, 26.1944285139720, -74.9689223087762, 160.994069793412, -271.116809361367, 367.666609439546, -406.316381735042, 367.666609439546, -271.116809361367, 160.994069793412, -74.9689223087762, 26.1944285139720, -6.22565762049704, 0.809066862358596}; 108 | std::vector expected_a{1, -7.46207919232620, 30.4441268607578, -84.4951944910069, 175.975358068302, -287.432627049382, 378.108892303000, -405.377130759820, 355.900301944903, -254.658587105406, 146.751918266687, -66.3241915216776, 22.4931625871694, -5.18935019671270, 0.654589187766792}; 109 | std::vector results{0, 0.809066862358596, 1.42979710302688, 2.20823887531852, 3.29062017183502, 4.54433190080803, 5.72722677360764, 6.71090557266292}; 110 | IIRFilter<7, FreqProperty::BandStop> flt(0.3, 0.33, false); 111 | EXPECT_TRUE(compare_vector(expected_a, flt.coff_a())); 112 | EXPECT_TRUE(compare_vector(expected_b, flt.coff_b())); 113 | test_filter(flt, datas2, results); 114 | } 115 | 116 | TEST_F(SIG_TestCase, IIRFilter_BP) 117 | { 118 | std::vector expected_b{1.22964988731449e-06, 0, -8.60754921120146e-06, 0, 2.58226476336044e-05, 0, -4.30377460560073e-05, 0, 4.30377460560073e-05, 0, -2.58226476336044e-05, 0, 8.60754921120146e-06, 0, -1.22964988731449e-06}; 119 | std::vector expected_a{1, -5.78651995575604, 19.9640294916800, -47.7010481156334, 87.8396722590763, -128.653305217857, 154.475693506059, -153.050400350683, 126.160440824548, -85.8045460115290, 47.8358196887074, -21.2060646062098, 7.24441026706683, -1.71334929459713, 0.241873198978882}; 120 | std::vector results{0, 1.22964988731449e-06, 9.57469338616748e-06, 2.59367881852580e-05, 5.29337085399868e-06, -0.000132313491922598, -0.000292346101087556, -3.40707974209247e-05}; 121 | IIRFilter<7, FreqProperty::BandPass> flt(0.3, 0.4, false); 122 | EXPECT_TRUE(compare_vector(expected_a, flt.coff_a())); 123 | EXPECT_TRUE(compare_vector(expected_b, flt.coff_b())); 124 | test_filter(flt, datas2, results); 125 | } 126 | 127 | TEST_F(SIG_TestCase, MovAvgFlt) 128 | { 129 | MovAvgFilter<2> flt1; 130 | std::vector expects{0, 1.0, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5}; 131 | test_filter(flt1, datas2, expects); 132 | 133 | MovAvgFilter<2> flt2(false); 134 | expects = {0, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5}; 135 | test_filter(flt2, datas2, expects); 136 | 137 | MovAvgFilter<5> flt3; 138 | expects = {1, 2, 3, 4, 5, 4, 5, 6}; 139 | test_filter(flt3, {1, 2, 3, 4, 5, 6, 7, 8}, expects); 140 | 141 | MovAvgFilter<5> flt4(false); 142 | expects = {0.2, 0.6, 1.2, 2, 3, 4, 5, 6}; 143 | test_filter(flt4, {1, 2, 3, 4, 5, 6, 7, 8}, expects); 144 | } -------------------------------------------------------------------------------- /test/solver_test.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include "liegroup.hpp" 3 | 4 | using namespace ppx; 5 | 6 | constexpr auto EQ_THS = EPS_SP * 5e2; 7 | 8 | class LinEqn_TestCase : public ::testing::Test 9 | { 10 | public: 11 | LinEqn_TestCase() = default; 12 | }; 13 | 14 | TEST_F(LinEqn_TestCase, Quadtratic) 15 | { 16 | auto res = quadsolve(1.0, -2 * 12345678, -1); 17 | EXPECT_EQ(res.s, StatusCode::NORMAL); 18 | EXPECT_NEAR(res.x[1] / -4.050000332100021e-08, 1, 1e-7); 19 | } 20 | 21 | TEST_F(LinEqn_TestCase, MGS) 22 | { 23 | MatrixS<3, 3> A{ 24 | {{1.0001, 0.5000, 0.3333}, 25 | {0.5000, 0.3334, 0.2500}, 26 | {0.3333, 0.2500, 0.2001}}, 27 | Ori::Row}; 28 | auto Q = MGS(A); 29 | auto residual = norm1<3, 3>(Q.T() * Q - eye<3>()); 30 | EXPECT_TRUE(residual < EPS_SP); 31 | MatrixS<100, 100> B; 32 | for (size_t i = 0; i < 100; i++) 33 | { 34 | random(B); 35 | auto R = MGS(B); 36 | residual = norm1<3, 3>(R.T() * R - eye<100>()); 37 | EXPECT_TRUE(residual < EPS_SP); 38 | } 39 | } 40 | 41 | TEST_F(LinEqn_TestCase, Matrix_Norm2) 42 | { 43 | MatrixS<5, 4> A{{0.814723686393179, 44 | 0.905791937075619, 45 | 0.126986816293506, 46 | 0.913375856139019, 47 | 0.632359246225410}, 48 | {0.0975404049994095, 49 | 0.278498218867048, 50 | 0.546881519204984, 51 | 0.957506835434298, 52 | 0.964888535199277}, 53 | {0.157613081677548, 54 | 0.970592781760616, 55 | 0.957166948242946, 56 | 0.485375648722841, 57 | 0.800280468888800}, 58 | {0.141886338627215, 59 | 0.421761282626275, 60 | 0.915735525189067, 61 | 0.792207329559554, 62 | 0.959492426392903}}; 63 | EXPECT_NEAR(norm2(A), 2.993584186569183, 10 * EPS_DP); 64 | MatrixS<4, 5> B{ 65 | {0.655740699156587, 66 | 0.0357116785741896, 67 | 0.849129305868777, 68 | 0.933993247757551}, 69 | {0.678735154857774, 70 | 0.757740130578333, 71 | 0.743132468124916, 72 | 0.392227019534168}, 73 | {0.655477890177557, 74 | 0.171186687811562, 75 | 0.706046088019609, 76 | 0.0318328463774207}, 77 | {0.276922984960890, 78 | 0.0461713906311539, 79 | 0.0971317812358475, 80 | 0.823457828327293}, 81 | {0.694828622975817, 82 | 0.317099480060861, 83 | 0.950222048838355, 84 | 0.0344460805029088}}; 85 | EXPECT_NEAR(norm2(B), 2.386200992922722, 10 * EPS_DP); 86 | MatrixS<4, 4> C{ 87 | {0.438744359656398, 88 | 0.381558457093008, 89 | 0.765516788149002, 90 | 0.795199901137063}, 91 | {0.186872604554379, 92 | 0.489764395788231, 93 | 0.445586200710900, 94 | 0.646313010111265}, 95 | {0.709364830858073, 96 | 0.754686681982361, 97 | 0.276025076998578, 98 | 0.679702676853675}, 99 | {0.655098003973841, 100 | 0.162611735194631, 101 | 0.118997681558377, 102 | 0.498364051982143}}; 103 | EXPECT_NEAR(norm2(C), 2.075457904661895, 10 * EPS_DP); 104 | } 105 | 106 | TEST_F(LinEqn_TestCase, LU_decompose) 107 | { 108 | MatrixS<100, 100> A; 109 | MatrixS<100, 1> x; 110 | for (size_t i = 0; i < 100; i++) 111 | { 112 | random(A, -1e5, 1e5); 113 | random(x, -1e5, 1e5); 114 | auto b = A * x; 115 | auto result = linsolve(A, b); 116 | if (result.s != StatusCode::SINGULAR) 117 | { 118 | auto residual = norm2<100, 1>(result.x - x); 119 | EXPECT_LE(residual, EQ_THS); 120 | } 121 | } 122 | } 123 | 124 | TEST_F(LinEqn_TestCase, QR_decompose) 125 | { 126 | MatrixS<100, 100> A; 127 | MatrixS<100, 1> x; 128 | 129 | for (size_t i = 0; i < 100; i++) 130 | { 131 | random(A, -1e5, 1e5); 132 | random(x, -1e5, 1e5); 133 | auto b = A * x; 134 | auto result = linsolve(A, b); 135 | if (result.s != StatusCode::SINGULAR) 136 | { 137 | auto residual = norm2<100, 1>(result.x - x); 138 | EXPECT_LE(residual, EQ_THS); 139 | } 140 | } 141 | } 142 | 143 | TEST_F(LinEqn_TestCase, SVD_decompose) 144 | { 145 | MatrixS<100, 100> A; 146 | MatrixS<100, 1> x; 147 | 148 | for (size_t i = 0; i < 100; i++) 149 | { 150 | random(A, -1e5, 1e5); 151 | random(x, -1e5, 1e5); 152 | auto b = A * x; 153 | auto result = linsolve(A, b); 154 | if (result.s != StatusCode::SINGULAR) 155 | { 156 | auto residual = norm2<100, 1>(result.x - x); 157 | EXPECT_LE(residual, EQ_THS); 158 | } 159 | } 160 | } 161 | -------------------------------------------------------------------------------- /test/statistic_test.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include "statistics.hpp" 3 | 4 | using namespace ppx; 5 | 6 | class STA_TestCase : public ::testing::Test 7 | { 8 | public: 9 | STA_TestCase() 10 | { 11 | v2d_1 = { 12 | 4.3236608, 3.35122338, 3.49222363, 3.69997592, 2.34909379, 2.18767488, 13 | 2.16444712, 3.42335775, 3.12684526, 5.29987833, 2.92278154, 3.78734002, 14 | 4.42159924, 2.87413605, 4.07878205, 3.78256866, 4.79076022, 2.90930839, 15 | 2.61726172, 2.05885, 4.6771387, 4.19422838, 3.45202478, 3.69340157, 16 | 3.19664012, 4.63889042, 2.62521637, 3.62217656, 5.46889539, 3.53423069, 17 | 4.76992267, 4.60783836, 3.42811992, 4.59510029, 2.86375389, 4.15423067, 18 | 5.76076757, 3.7043695, 4.9028766, 2.87812984, 2.32072277, 3.07481847, 19 | 4.12073955, 3.33009106, 3.58941333, 3.81311256, 2.62879508, 3.32717704, 20 | 4.80311367, 4.46290715, 3.27371626, 3.98017632, 5.19831798, 2.25918325, 21 | 2.58106533, 4.64115646, 3.63653241, 4.79635885, 4.29438109, 4.09783039, 22 | 1.23649083, 3.31382716, 4.38057054, 5.56158072, 5.29198396, 4.02055902, 23 | 3.44527608, 5.31117144, 4.19895772, 3.67044897, 3.64422719, 3.09757485, 24 | 5.29702108, 3.2517827, 3.88572179, 1.6041799, 4.3455102, 4.33209092, 25 | 3.76069677, 3.94913431, 2.55490818, 4.00148904, 4.19922533, 3.19609868, 26 | 2.79493504, 2.85257333, 4.51558046, 2.88748332, 4.15717856, 3.21719062, 27 | 3.05412371, 4.8201182, 3.85739444, 3.41214923, 4.87697885, 2.57467735, 28 | 4.72403246, 3.45126226, 4.95347615, 3.08640421}; 29 | v2d_2 = { 30 | -1.42098395, -1.33204635, -2.8580657, -2.19562405, -2.3707501, -0.23669885, 31 | -0.85638882, -3.21767737, -4.57052057, -2.9399555, -2.2117331, -3.01265573, 32 | -1.55902518, -0.23236244, -2.20996735, 0.8106878, -1.2004983, -1.71195628, 33 | -2.278503, -2.75240968, -1.99841232, -0.91895005, -2.1703771, -1.06907065, 34 | -3.45958206, -2.05024332, -3.63932494, -4.67213955, -1.04179572, -3.77907907, 35 | -1.82145315, -3.84594605, -2.61303378, -2.40153486, -1.61567927, -0.4107063, 36 | -2.77427322, -2.874331, 0.46227023, -3.18169647, -3.53009246, -0.77510894, 37 | -3.11596277, -3.9312921, -1.68218639, -0.62131798, -2.33221182, -2.93372489, 38 | -3.31757796, -2.14576392, -1.94223681, -3.74269179, 1.33332551, -3.56772704, 39 | -3.51990906, -1.71581284, -3.24915184, -0.87708048, -1.39255409, -1.22711598, 40 | -2.72349569, -1.26613809, -3.01723601, -1.89956282, -1.11987124, -2.88416298, 41 | -0.74773852, -0.85496716, -2.12308703, -1.21017992, -1.56354312, -0.97158979, 42 | -1.66461298, -0.72439225, -3.15352057, -2.53907694, -1.59750827, -1.57770646, 43 | -1.23775326, -0.46525053, -3.13243791, -1.82909815, -0.18621321, -3.68267914, 44 | -4.83941495, -4.05554548, -1.41165984, -3.52446256, -2.98693379, -1.67824542, 45 | -2.03616993, -2.12928829, -1.77796807, -2.09979684, -1.00371755, -2.66780956, 46 | -1.3404693, -2.0590144, -1.07913955, -1.72356594}; 47 | 48 | gmr_1 = { 49 | 0., 0.06346652, 0.12693304, 0.19039955, 0.25386607, 0.31733259, 50 | 0.38079911, 0.44426563, 0.50773215, 0.57119866, 0.63466518, 0.6981317, 51 | 0.76159822, 0.82506474, 0.88853126, 0.95199777, 1.01546429, 1.07893081, 52 | 1.14239733, 1.20586385, 1.26933037, 1.33279688, 1.3962634, 1.45972992, 53 | 1.52319644, 1.58666296, 1.65012947, 1.71359599, 1.77706251, 1.84052903, 54 | 1.90399555, 1.96746207, 2.03092858, 2.0943951, 2.15786162, 2.22132814, 55 | 2.28479466, 2.34826118, 2.41172769, 2.47519421, 2.53866073, 2.60212725, 56 | 2.66559377, 2.72906028, 2.7925268, 2.85599332, 2.91945984, 2.98292636, 57 | 3.04639288, 3.10985939, 3.17332591, 3.23679243, 3.30025895, 3.36372547, 58 | 3.42719199, 3.4906585, 3.55412502, 3.61759154, 3.68105806, 3.74452458, 59 | 3.8079911, 3.87145761, 3.93492413, 3.99839065, 4.06185717, 4.12532369, 60 | 4.1887902, 4.25225672, 4.31572324, 4.37918976, 4.44265628, 4.5061228, 61 | 4.56958931, 4.63305583, 4.69652235, 4.75998887, 4.82345539, 4.88692191, 62 | 4.95038842, 5.01385494, 5.07732146, 5.14078798, 5.2042545, 5.26772102, 63 | 5.33118753, 5.39465405, 5.45812057, 5.52158709, 5.58505361, 5.64852012, 64 | 5.71198664, 5.77545316, 5.83891968, 5.9023862, 5.96585272, 6.02931923, 65 | 6.09278575, 6.15625227, 6.21971879, 6.28318531}; 66 | gmr_2 = { 67 | 0.01440436, 0.20885127, 0.20269623, 0.20141875, 0.29553431, 0.34540088, 68 | 0.52107036, 0.40927909, 0.51750351, 0.45523124, 0.33760895, 0.70814947, 69 | 0.77652263, 0.66037521, 1.00312193, 0.66913938, 0.85430128, 0.86273498, 70 | 1.06290992, 1.08108374, 0.97039698, 1.00962782, 0.89602918, 0.79575882, 71 | 0.96407612, 1.01550902, 1.11988384, 1.11005943, 0.94006976, 0.93361188, 72 | 0.84014552, 0.7803525, 0.72536676, 1.06110294, 0.78160464, 0.75195441, 73 | 0.63047004, 0.79044321, 0.50537922, 0.59688496, 0.47751321, 0.55236764, 74 | 0.40714601, 0.28286732, 0.33920192, 0.32456574, 0.22696226, 0.18824859, 75 | 0.03162383, -0.00454618, -0.09897398, -0.13101136, -0.23931602, -0.39293879, 76 | -0.26398994, -0.38219824, -0.56395037, -0.4119483, -0.60440723, -0.56186532, 77 | -0.54524993, -0.65387071, -0.5987541, -0.87923216, -0.75552768, -0.90105086, 78 | -0.95310512, -0.95387874, -0.95350955, -0.93938428, -1.08035714, -0.8887198, 79 | -0.9432552, -1.15047914, -0.85104891, -0.80927842, -0.87596051, -1.00280024, 80 | -1.07888683, -0.84945707, -0.97446555, -0.78738749, -0.86062587, -0.75206153, 81 | -0.77893931, -0.70548915, -0.73354171, -0.51149196, -0.6300964, -0.55270899, 82 | -0.35232575, -0.62097264, -0.55684341, -0.27472278, -0.42934579, -0.05678587, 83 | -0.23061314, -0.20133793, 0.12887028, 0.14805148}; 84 | } 85 | std::vector v2d_1, v2d_2; 86 | std::vector gmr_1, gmr_2; 87 | }; 88 | 89 | TEST_F(STA_TestCase, 2DGaussianDist) 90 | { 91 | MVN<2> dist; 92 | MVN<2>::samples sa; 93 | for (size_t i = 0; i < std::min(v2d_1.size(), v2d_2.size()); i++) 94 | { 95 | sa.push_back(MatrixS<2, 1>{v2d_1[i], v2d_2[i]}); 96 | } 97 | 98 | dist.fit(sa); 99 | auto mean_err = norm2<2, 1>(dist.mean() - MatrixS<2, 1>{3.9, -2.0}); 100 | auto cov_err = norm2<2, 2>(dist.covariance() - MatrixS<2, 2>{0.9, 0.3, 0.3, 1.4}); 101 | EXPECT_NEAR(mean_err, 0.0, 0.2); 102 | EXPECT_NEAR(cov_err, 0.0, 0.08); 103 | } 104 | 105 | TEST_F(STA_TestCase, GMR_2D) 106 | { 107 | GMM<2, 3> gmm; 108 | GMM<2, 3>::samples sa; 109 | std::vector> knowns; 110 | for (size_t i = 0; i < std::min(gmr_1.size(), gmr_2.size()); i++) 111 | { 112 | sa.push_back(MatrixS<2, 1>{gmr_1[i], gmr_2[i]}); 113 | knowns.push_back(MatrixS<1, 1>{gmr_1[i]}); 114 | } 115 | 116 | gmm.setcomp(0, MVN<2>({1, 1}, {1, 0, 0, 1}), 0.3); 117 | gmm.setcomp(1, MVN<2>({2.5, 0.0}, {1, 0, 0, 1}), 0.4); 118 | gmm.setcomp(2, MVN<2>({5.5, -0.5}, {1, 0, 0, 1}), 0.3); 119 | 120 | gmm.fit(sa); 121 | std::cout << gmm << std::endl; 122 | auto pred = gmm.predict(knowns, {0}); 123 | for (size_t i = 0; i < pred.size(); i++) 124 | { 125 | std::cout << gmr_1[i] << " " << pred[i][0] << " " << gmr_2[i] << "\n"; 126 | } 127 | } 128 | 129 | TEST_F(STA_TestCase, GMR_Linear) 130 | { 131 | std::vector x{0., 0.6981317, 1.3962634, 2.0943951, 2.7925268, 3.4906585, 4.1887902, 4.88692191, 5.58505361, 6.28318531}; 132 | std::vector y{1.58565603, 0.66114222, -5.06753345, -6.33437884, -5.364985, -10.50269171, -10.51272332, -12.92031932, -15.34471305, -17.9232931}; 133 | GMM<2, 2> gmm; 134 | GMM<2, 2>::samples sa; 135 | for (size_t i = 0; i < std::min(x.size(), y.size()); i++) 136 | { 137 | sa.push_back(MatrixS<2, 1>{x[i], y[i]}); 138 | } 139 | 140 | gmm.setcomp(0, MVN<2>({1, -5}, {1, 0, 0, 1}), 0.5); 141 | gmm.setcomp(1, MVN<2>({2.5, -7.0}, {1, 0, 0, 1}), 0.5); 142 | 143 | gmm.fit(sa); 144 | std::cout << gmm << std::endl; 145 | 146 | std::vector> knowns; 147 | for (size_t i = 0; i < 100; i++) 148 | { 149 | knowns.push_back(MatrixS<1, 1>{0.0628 * i}); 150 | } 151 | 152 | auto pred = gmm.predict(knowns, {0}); 153 | for (size_t i = 0; i < pred.size(); i++) 154 | { 155 | std::cout << knowns[i][0] << " " << pred[i][0] << "\n"; 156 | } 157 | } -------------------------------------------------------------------------------- /tools/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Xtinc/matrix/e363e2437fdabef0df5e519486c2f4e4c2f7b45b/tools/__init__.py -------------------------------------------------------------------------------- /tools/gnuplot.mplstyle: -------------------------------------------------------------------------------- 1 | ## Axes 2 | axes.titlesize: large 3 | axes.labelsize: large 4 | axes.formatter.use_mathtext: True 5 | axes.formatter.limits: -4, 4 6 | axes.grid: True # display grid or not 7 | axes.prop_cycle: cycler('color', ['5d81b4','e09b24','8eb031','eb6235','8678b2','c46e1a','5c9dc7','ffbf00','a5609c']) 8 | axes3d.xaxis.panecolor: (1.0, 1.0, 1.0, 1.0) 9 | axes3d.yaxis.panecolor: (1.0, 1.0, 1.0, 1.0) 10 | axes3d.zaxis.panecolor: (1.0, 1.0, 1.0, 1.0) 11 | 12 | ## Lines 13 | lines.linewidth: 1.0 14 | lines.markersize: 4 15 | hatch.linewidth: 0.25 16 | 17 | patch.antialiased: True 18 | 19 | ## Ticks 20 | xtick.top: True 21 | xtick.bottom: True 22 | xtick.major.size: 4.0 23 | xtick.minor.size: 2.0 24 | xtick.major.width: 0.5 25 | xtick.minor.width: 0.5 26 | xtick.direction: in 27 | xtick.minor.visible: True 28 | xtick.major.top: True 29 | xtick.major.bottom: True 30 | xtick.minor.top: True 31 | xtick.minor.bottom: True 32 | xtick.major.pad: 5.0 33 | xtick.minor.pad: 5.0 34 | ytick.labelsize: large 35 | 36 | ytick.left: True 37 | ytick.right: True 38 | ytick.major.size: 4.0 39 | ytick.minor.size: 2.0 40 | ytick.major.width: 0.5 41 | ytick.minor.width: 0.5 42 | ytick.direction: in 43 | ytick.minor.visible: True 44 | ytick.major.left: True 45 | ytick.major.right: True 46 | ytick.minor.left: True 47 | ytick.minor.right: True 48 | ytick.major.pad: 5.0 49 | ytick.minor.pad: 5.0 50 | xtick.labelsize: large 51 | 52 | ## Legend 53 | legend.frameon: True 54 | legend.fontsize: 10 55 | legend.framealpha: 1.0 56 | legend.edgecolor: 1.0 57 | 58 | ## Figure size 59 | figure.figsize: 6.4, 4.8 60 | figure.dpi: 120 61 | figure.autolayout: False 62 | 63 | ### Fonts 64 | mathtext.fontset: cm 65 | font.family: serif 66 | font.serif: cmr10 67 | 68 | ### GRIDS 69 | grid.color: gray 70 | grid.linestyle: dotted # solid 71 | grid.linewidth: 0.5 # in points 72 | grid.alpha: 0.5 # transparency, between 0.0 and 1.0 73 | 74 | savefig.dpi: 180 # figure dots per inch or 'figure' -------------------------------------------------------------------------------- /tools/misc.py: -------------------------------------------------------------------------------- 1 | """A cluster of miscellaneous things. 2 | 3 | Contains: 4 | Packed FFT function. 5 | """ 6 | from collections import deque 7 | import numpy as np 8 | from scipy.fft import fft as scipyfft 9 | from scipy.signal import welch as scipywelch 10 | from pytransform3d.plot_utils import Frame 11 | 12 | 13 | def fft(x_s, f_s): 14 | """Calculate FFT of sequence xs, fs is sampling frequence.""" 15 | idx = int(np.log2(len(x_s))) 16 | seq_x = x_s[1 : int(np.exp2(idx))] 17 | data_len = len(seq_x) 18 | half_len = int(data_len / 2) 19 | fft_x = np.array(scipyfft(seq_x, norm="forward")) 20 | amp_x = abs(fft_x) * 2 21 | lable_x = np.linspace(0, half_len - 1, half_len) 22 | amp_half = amp_x[0:half_len] 23 | amp_half[0] = 0 24 | fre = lable_x / data_len * f_s 25 | pha = np.unwrap(np.angle(fft_x)) 26 | return fre, amp_half, pha 27 | 28 | 29 | def psd(x_s, f_s, scale_type="density"): 30 | """Calculate PSD by welch Periodogram, fs is sampling frequence.""" 31 | idx = int(np.log2(len(x_s))) 32 | if scale_type == "density": 33 | return scipywelch(x_s, f_s, nperseg=int(np.exp2(idx)), scaling="density") 34 | return scipywelch( 35 | x_s, f_s, "flattop", nperseg=int(np.exp2(idx)), scaling="spectrum" 36 | ) 37 | 38 | 39 | class VisualPose: 40 | """Store Transform and trajectory for plot. 41 | 42 | func: function which will be called to update transform. 43 | """ 44 | 45 | def __init__(self, axis3d, trans, traj_len: int = 50, scale=1.0): 46 | self.__trans = trans 47 | self.__frame = Frame(self.__trans, s=scale) 48 | self.__frame.add_frame(axis3d) 49 | 50 | self.__px = deque([self.__trans[0, 3]], maxlen=traj_len) 51 | self.__py = deque([self.__trans[1, 3]], maxlen=traj_len) 52 | self.__pz = deque([self.__trans[2, 3]], maxlen=traj_len) 53 | 54 | self.__line = axis3d.plot(self.__px, self.__py, self.__pz, c="r", alpha=0.2)[0] 55 | 56 | @property 57 | def trans(self): 58 | """return transform now.""" 59 | return self.__trans 60 | 61 | def __iter__(self): 62 | return ( 63 | (self.__px[idx], self.__py[idx], self.__pz[idx]) 64 | for idx in range(len(self.__px)) 65 | ) 66 | 67 | def __str__(self) -> str: 68 | return str(tuple(self)) 69 | 70 | def update(self, trans): 71 | """update transform by single parameter.""" 72 | self.__trans = trans 73 | self.__frame.set_data(self.__trans) 74 | self.__px.append(self.__trans[0, 3]) 75 | self.__py.append(self.__trans[1, 3]) 76 | self.__pz.append(self.__trans[2, 3]) 77 | self.__line.set_data(self.__px, self.__py) 78 | self.__line.set_3d_properties(self.__pz) 79 | 80 | def clear(self): 81 | """clear trajectory.""" 82 | self.__frame.set_data(self.__trans) 83 | for pos in (self.__px, self.__py, self.__pz): 84 | pos.clear() 85 | 86 | 87 | class VisualLink: 88 | """plot linkage between frame""" 89 | 90 | def __init__(self, axis3d, obj1: VisualPose, obj2: VisualPose): 91 | self.__line = axis3d.plot( 92 | (obj1.trans[0, 3], obj2.trans[0, 3]), 93 | (obj1.trans[1, 3], obj2.trans[1, 3]), 94 | (obj1.trans[2, 3], obj2.trans[2, 3]), 95 | c="k", 96 | )[0] 97 | self.__objs = obj1 98 | self.__obje = obj2 99 | 100 | def update(self): 101 | """update linkage with transform.""" 102 | self.__line.set_data( 103 | (self.__objs.trans[0, 3], self.__obje.trans[0, 3]), 104 | (self.__objs.trans[1, 3], self.__obje.trans[1, 3]), 105 | ) 106 | self.__line.set_3d_properties( 107 | (self.__objs.trans[2, 3], self.__obje.trans[2, 3]) 108 | ) 109 | --------------------------------------------------------------------------------