├── .checkpatch.conf ├── .clang-format ├── .clang-tidy ├── .github └── workflows │ └── ci.yml ├── .gitignore ├── .gitlab-ci.yml ├── CMakeLists.txt ├── CODEOWNERS ├── LICENSE ├── README.md ├── cmake ├── clang-tidy.cmake └── configure_tidy.cmake ├── data ├── HangingLoad.csv ├── MotorRotation.csv └── MultivariableCylinders.csv ├── doc ├── CMakeLists.txt ├── Doxyfile.in ├── ai │ ├── a_star.adoc │ ├── index.adoc │ └── inpolygon.adoc ├── contents.toml.in ├── dynamics │ ├── c2d.adoc │ ├── index.adoc │ ├── kalman.adoc │ ├── lqi.adoc │ ├── mpc.adoc │ ├── mrac.adoc │ ├── pid.adoc │ ├── stability.adoc │ └── theta2ss.adoc ├── filter │ ├── filtfilt.adoc │ ├── index.adoc │ ├── mcs.adoc │ └── sqr_ukf.adoc ├── img │ ├── motion_profile_1.jpg │ ├── motion_profile_2.jpg │ ├── motion_profile_3.jpg │ └── motion_profile_4.jpg ├── index.adoc ├── linalg │ ├── add.adoc │ ├── balance.adoc │ ├── chol.adoc │ ├── cholupdate.adoc │ ├── det.adoc │ ├── dlyap.adoc │ ├── eig.adoc │ ├── eig_sym.adoc │ ├── expm.adoc │ ├── hankel.adoc │ ├── index.adoc │ ├── inv.adoc │ ├── linsolve_chol.adoc │ ├── linsolve_gauss.adoc │ ├── linsolve_lower_triangular.adoc │ ├── linsolve_lup.adoc │ ├── linsolve_markov.adoc │ ├── linsolve_qr.adoc │ ├── linsolve_upper_triangular.adoc │ ├── lup.adoc │ ├── mul.adoc │ ├── nonlinsolve.adoc │ ├── norm.adoc │ ├── pinv.adoc │ ├── qr.adoc │ ├── sum.adoc │ ├── svd_golub_reinsch.adoc │ ├── svd_jacobi_one_sided.adoc │ └── tran.adoc ├── misc │ ├── cat.adoc │ ├── constrain.adoc │ ├── cut.adoc │ ├── index.adoc │ ├── insert.adoc │ ├── mean.adoc │ ├── print.adoc │ ├── randn.adoc │ ├── sign.adoc │ └── stddev.adoc ├── model │ ├── dc_motor.adoc │ └── index.adoc ├── motion │ ├── index.adoc │ └── motion_profile.adoc ├── motor │ ├── clarke.adoc │ ├── index.adoc │ └── park.adoc ├── optimization │ ├── index.adoc │ └── linprog.adoc ├── packages.toml.in ├── sysid │ ├── index.adoc │ ├── okid_era.adoc │ ├── rls.adoc │ └── sqr_ukf_id.adoc └── theme │ ├── README.adoc │ ├── Rakefile │ ├── fonts │ ├── ClearSans-Bold.ttf │ ├── ClearSans-BoldItalic.ttf │ ├── ClearSans-Italic.ttf │ ├── ClearSans-LICENSE-2.0.txt │ ├── ClearSans-Light.ttf │ ├── ClearSans-Medium.ttf │ ├── ClearSans-MediumItalic.ttf │ ├── ClearSans-Regular.ttf │ ├── ClearSans-Thin.ttf │ ├── Courier Prime Sans Bold Italic.ttf │ ├── Courier Prime Sans Bold.ttf │ ├── Courier Prime Sans Italic.ttf │ ├── Courier Prime Sans.ttf │ └── CourierPrimeSans-LICENSE.md │ ├── icons │ ├── .gitignore │ └── .placeholder │ ├── images │ └── .gitignore │ └── tutorial.yml ├── include └── control │ ├── ai.h │ ├── dynamics.h │ ├── filter.h │ ├── linalg.h │ ├── misc.h │ ├── model.h │ ├── model │ └── dc_motor.h │ ├── motion.h │ ├── motor.h │ ├── optimization.h │ └── sysid.h ├── integrity ├── DocChecker.py ├── ai.robot ├── dynamics.robot ├── filter.robot ├── linalg.robot ├── misc.robot ├── model.robot ├── module.robot ├── motion.robot ├── motor.robot ├── optimization.robot ├── spdx.robot ├── sysid.robot └── top-level.robot ├── octave ├── control │ ├── README.md │ ├── acker.m │ ├── allmargin.m │ ├── append.m │ ├── are.m │ ├── balreal.m │ ├── bode.m │ ├── bodemag.m │ ├── c2d.m │ ├── c2dt.m │ ├── covar.m │ ├── ctrb.m │ ├── d2c.m │ ├── d2d.m │ ├── damp.m │ ├── db2mag.m │ ├── dbdrop.m │ ├── dcgain.m │ ├── evalfr.m │ ├── feedback.m │ ├── findmaxgain.m │ ├── freqresp.m │ ├── gensig.m │ ├── gram.m │ ├── hsvd.m │ ├── imc.m │ ├── impulse.m │ ├── initial.m │ ├── lmpc.m │ ├── loop.m │ ├── lqe.m │ ├── lqgreg.m │ ├── lqi.m │ ├── lqr.m │ ├── lsim.m │ ├── lyap.m │ ├── mag2db.m │ ├── margin.m │ ├── minreal.m │ ├── modred.m │ ├── nlsim.m │ ├── nyquist.m │ ├── obsv.m │ ├── pade.m │ ├── parallel.m │ ├── pid.m │ ├── pipd.m │ ├── pole.m │ ├── pzmap.m │ ├── ramp.m │ ├── referencegain.m │ ├── reg.m │ ├── rlocfind.m │ ├── rlocus.m │ ├── satlsim.m │ ├── sensitivity.m │ ├── series.m │ ├── sgrid.m │ ├── sigma.m │ ├── smithpredict.m │ ├── ss.m │ ├── ss2tf.m │ ├── step.m │ ├── tf.m │ ├── tf2ss.m │ ├── tzero.m │ ├── zero.m │ └── zpk.m └── systemid │ ├── eradc.m │ ├── filtfilt2.m │ ├── ica.m │ ├── idbode.m │ ├── moesp.m │ ├── n4sid.m │ ├── ocid.m │ ├── okid.m │ ├── pimoesp.m │ ├── rls.m │ ├── rpca.m │ ├── sindy.m │ ├── spa.m │ ├── sr_ukf_parameter_estimation.m │ ├── sr_ukf_state_estimation.m │ └── svm.m ├── pictures ├── AppliedSystemIdentification.jpeg ├── ERADC_Result.png ├── ERADC_System.png ├── FILTFILT2_Result.png ├── FestoBench.jpg ├── ICA_After.png ├── ICA_Before.png ├── ICA_Mixed_Signals.png ├── IDBODE_Result.png ├── IDBODE_System.png ├── Markering_024.png ├── OCID_Result.png ├── OCID_System.png ├── OKID_Result.png ├── OKID_System.jpg ├── PLC system.jpg ├── RLS_Result.png ├── RLS_System.jpg ├── RPCA_Bob.png ├── RolfJohanssonsBok.jpg ├── SINDY_Result.png ├── SINDY_Result_multivariable.png ├── SPA_Result.png ├── SR-UKF │ ├── sr-ukf-paper.pdf │ ├── sr-ukf-parameter-estimation.m │ ├── sr-ukf-parameter-estimation.png │ ├── sr-ukf-state-estimation.m │ └── sr-ukf-state-estimation.png ├── SR_UKF_parameter_estimation.png ├── SR_UKF_state_estimation.png ├── SVM_c_header.png ├── SVM_c_source.png ├── SVM_plot.png ├── SVM_results.png ├── Skärmbild från 2017-11-09 00-00-55.png ├── Skärmbild från 2017-11-09 00-01-11.png ├── Skärmbild från 2017-11-09 00-04-32.png ├── Skärmbild från 2017-11-09 00-04-59.png ├── Skärmbild från 2017-11-09 00-06-29.png ├── Skärmbild från 2017-11-09 00-06-41.png ├── Skärmbild från 2017-11-09 00-07-36.png ├── Skärmbild från 2017-11-09 00-08-23.png ├── Skärmbild från 2017-11-09 00-09-19.png ├── Skärmbild från 2017-11-09 00-10-03.png ├── Skärmbild från 2017-11-09 00-11-30.png ├── Skärmbild från 2017-11-09 00-12-02.png ├── cover.png └── examples.md ├── reports ├── Applied System Identification - Lecture note 1.pdf ├── Applied System Identification - Lecture note 2.pdf ├── Applied System Identification - Lecture note 3.pdf ├── ERADC.pdf ├── ICA.pdf ├── OCID.pdf ├── OKID.pdf ├── RLS.pdf ├── SINDY.pdf ├── SSFD.pdf └── sr-ukf-paper.pdf ├── results.txt ├── scripts ├── build ├── check ├── checkpatch.pl ├── checkpatch │ └── typedefsfile ├── ci │ ├── check-file-sorted │ ├── check-links │ ├── check_compliance.py │ ├── pylintrc │ ├── test_plan.py │ └── twister_ignore.txt ├── coverage ├── fix ├── init ├── sort-codeowners ├── spelling.txt └── test ├── src ├── CMakeLists.txt ├── ai │ ├── a_star.c │ └── inpolygon.c ├── dynamics │ ├── c2d.c │ ├── kalman.c │ ├── lqi.c │ ├── mpc.c │ ├── mrac.c │ ├── pid.c │ ├── pid.m │ ├── stability.c │ └── theta2ss.c ├── filter │ ├── filtfilt.c │ ├── mcs.c │ └── sqr_ukf.c ├── linalg │ ├── add.c │ ├── balance.c │ ├── chol.c │ ├── cholupdate.c │ ├── det.c │ ├── dlyap.c │ ├── eig.c │ ├── eig_sym.c │ ├── expm.c │ ├── hankel.c │ ├── inv.c │ ├── linsolve_chol.c │ ├── linsolve_gauss.c │ ├── linsolve_lower_triangular.c │ ├── linsolve_lup.c │ ├── linsolve_markov.c │ ├── linsolve_qr.c │ ├── linsolve_upper_triangular.c │ ├── lup.c │ ├── mul.c │ ├── nonlinsolve.c │ ├── norm.c │ ├── pinv.c │ ├── qr.c │ ├── sum.c │ ├── svd_golub_reinsch.c │ ├── svd_jacobi_one_sided.c │ └── tran.c ├── misc │ ├── cat.c │ ├── constrain.c │ ├── cut.c │ ├── insert.c │ ├── mean.c │ ├── print.c │ ├── randn.c │ ├── sign.c │ └── stddev.c ├── model │ ├── dc_motor.c │ └── dc_motor.m ├── motion │ └── motion_profile.c ├── motor │ ├── clarke.c │ └── park.c ├── optimization │ ├── linprog.c │ └── linprog.m └── sysid │ ├── okid_era.c │ ├── rls.c │ └── sqr_ukf_id.c ├── tests ├── CMakeLists.txt ├── ai │ ├── CMakeLists.txt │ ├── a_star.cpp │ ├── inpolygon.cpp │ └── main.cpp ├── dynamics │ ├── CMakeLists.txt │ ├── c2d.cpp │ ├── kalman.cpp │ ├── kalman.m │ ├── lqi.cpp │ ├── lqi.m │ ├── main.cpp │ ├── mpc.cpp │ ├── mrac.cpp │ ├── pid.cpp │ ├── pid.m │ ├── stability.cpp │ └── theta2ss.cpp ├── filter │ ├── CMakeLists.txt │ ├── filtfilt.cpp │ ├── main.cpp │ ├── mcs.cpp │ └── sqr_ukf.cpp ├── linalg │ ├── CMakeLists.txt │ ├── add.cpp │ ├── balance.cpp │ ├── chol.cpp │ ├── cholupdate.cpp │ ├── det.cpp │ ├── dlyap.cpp │ ├── eig.cpp │ ├── eig_sym.cpp │ ├── expm.cpp │ ├── hankel.cpp │ ├── inv.cpp │ ├── linsolve_chol.cpp │ ├── linsolve_gauss.cpp │ ├── linsolve_lower_triangular.cpp │ ├── linsolve_lup.cpp │ ├── linsolve_markov.cpp │ ├── linsolve_qr.cpp │ ├── linsolve_upper_triangular.cpp │ ├── lup.cpp │ ├── main.cpp │ ├── mul.cpp │ ├── nonlinsolve.cpp │ ├── norm.cpp │ ├── pinv.cpp │ ├── qr.cpp │ ├── sum.cpp │ ├── svd_golub_reinsch.cpp │ ├── svd_jacobi_one_sided.cpp │ └── tran.cpp ├── misc │ ├── CMakeLists.txt │ ├── cat.cpp │ ├── constrain.cpp │ ├── cut.cpp │ ├── insert.cpp │ ├── main.cpp │ ├── mean.cpp │ ├── print.cpp │ ├── randn.cpp │ ├── sign.cpp │ └── stddev.cpp ├── model │ ├── CMakeLists.txt │ ├── dc_motor.cpp │ └── main.cpp ├── motion │ ├── CMakeLists.txt │ ├── main.cpp │ └── motion_profile.cpp ├── motor │ ├── CMakeLists.txt │ ├── clarke.cpp │ ├── main.cpp │ └── park.cpp ├── optimization │ ├── CMakeLists.txt │ ├── linprog.cpp │ └── main.cpp └── sysid │ ├── CMakeLists.txt │ ├── main.cpp │ ├── okid_era.cpp │ ├── okid_era.m │ ├── rls.cpp │ └── sqr_ukf_id.cpp └── zephyr ├── CMakeLists.txt ├── Kconfig └── module.yml /.checkpatch.conf: -------------------------------------------------------------------------------- 1 | --emacs 2 | --summary-file 3 | --show-types 4 | --max-line-length=100 5 | --min-conf-desc-length=1 6 | --typedefsfile=scripts/checkpatch/typedefsfile 7 | 8 | --ignore LINE_SPACING 9 | --ignore DEEP_INDENTATION 10 | --ignore SPDX_LICENSE_TAG 11 | --ignore LEADING_SPACE 12 | --ignore CONSTANT_COMPARISON 13 | --ignore TYPO_SPELLING 14 | --ignore PREFER_ALIGNED 15 | --ignore PREFER_PACKED 16 | --ignore C99_COMMENTS 17 | --ignore BRACES 18 | --ignore PRINTK_WITHOUT_KERN_LEVEL 19 | --ignore SPLIT_STRING 20 | --ignore VOLATILE 21 | --ignore SPACING 22 | --ignore CONFIG_EXPERIMENTAL 23 | --ignore PREFER_KERNEL_TYPES 24 | --ignore PREFER_SECTION 25 | --ignore AVOID_EXTERNS 26 | --ignore NETWORKING_BLOCK_COMMENT_STYLE 27 | --ignore DATE_TIME 28 | --ignore MINMAX 29 | --ignore CONST_STRUCT 30 | --ignore FILE_PATH_CHANGES 31 | --ignore C99_COMMENT_TOLERANCE 32 | --ignore REPEATED_WORD 33 | --ignore UNDOCUMENTED_DT_STRING 34 | --ignore DT_SPLIT_BINDING_PATCH 35 | --ignore DT_SCHEMA_BINDING_PATCH 36 | --ignore TRAILING_SEMICOLON 37 | --ignore COMPLEX_MACRO 38 | --ignore MULTISTATEMENT_MACRO_USE_DO_WHILE 39 | --ignore ENOSYS 40 | --ignore IS_ENABLED_CONFIG 41 | --exclude scripts/spelling.txt 42 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: [ main ] 6 | tags: [ 'v*' ] 7 | pull_request: 8 | branches: [ main ] 9 | 10 | concurrency: 11 | group: ${{ github.ref }} 12 | cancel-in-progress: true 13 | 14 | jobs: 15 | build: 16 | name: Build 17 | runs-on: ubuntu-20.04 18 | container: 19 | image: docker.io/swedishembedded/build:latest 20 | steps: 21 | - name: Prepare 22 | run: | 23 | apt-get update 24 | apt-get install git-lfs 25 | - name: Configure git to trust the workspace despite the different owner 26 | run: | 27 | git config --global --add safe.directory "$GITHUB_WORKSPACE" 28 | - name: Checkout 29 | uses: actions/checkout@v2 30 | with: 31 | fetch-depth: 0 32 | lfs: true 33 | - name: Build everything 34 | run: | 35 | ./scripts/init 36 | ./scripts/check 37 | ./scripts/build 38 | ./scripts/test 39 | - name: Upload artifacts 40 | uses: actions/upload-artifact@v3 41 | with: 42 | name: package 43 | path: | 44 | build/*.deb 45 | build/control.pdf 46 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore build artifacts 2 | /build*/ 3 | /release/ 4 | /twister-out*/ 5 | /html*/ 6 | /latex*/ 7 | /CMakeCache.txt 8 | /doc/_build/ 9 | *.so 10 | 11 | # Ignore test artifacts 12 | /log.html 13 | /report.html 14 | /output.xml 15 | 16 | # Ignore output of compliance checks 17 | /Codeowners.txt 18 | /Gitlint.txt 19 | /Identity.txt 20 | /Kconfig.txt 21 | /KconfigBasic.txt 22 | /Nits.txt 23 | /checkpatch.txt 24 | /compliance.xml 25 | /Codeowners.txt 26 | /pylint.txt 27 | 28 | # Ignore gdb history 29 | /.gdb_history 30 | 31 | # Ignore editor swap files (vim, emacs etc) 32 | *.swp 33 | *~ 34 | 35 | # Ignore python environment 36 | .venv 37 | __pycache__/ 38 | 39 | # Ignore generated west config 40 | /.west/config 41 | 42 | # CScope & Tags 43 | /cscope.files 44 | /cscope.out* 45 | /TAGS 46 | 47 | # VSCode 48 | /.vscode 49 | 50 | # Robot framework files 51 | /logs/ 52 | /results*.robot.xml 53 | /robot_output.xml 54 | /snapshots/ 55 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | image: swedishembedded/build:v0.24.6 2 | 3 | default: 4 | tags: 5 | - docker 6 | 7 | stages: 8 | - build 9 | 10 | # This job builds applications 11 | build: 12 | stage: build 13 | artifacts: 14 | paths: 15 | - build/*.deb 16 | - build/control.pdf 17 | expire_in: 1 week 18 | when: always 19 | rules: 20 | - if: $CI_PIPELINE_SOURCE == "merge_request_event" 21 | script: 22 | - ./scripts/init 23 | - ./scripts/check 24 | - ./scripts/build 25 | - ./scripts/test 26 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: MIT 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/consulting 4 | # Training: https://swedishembedded.com/training 5 | # This is a standalone CMake file for use without Zephyr 6 | 7 | cmake_minimum_required(VERSION 3.12.2) 8 | 9 | project (control VERSION 0.34.0) 10 | 11 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 12 | include(configure_tidy) 13 | include(CTest) 14 | 15 | option(SECONTROL_CLANG_TIDY "Build with clang-tidy static analysis" OFF) 16 | 17 | add_subdirectory(doc) 18 | add_subdirectory(src) 19 | add_subdirectory(tests) 20 | 21 | set(CPACK_DEBIAN_PACKAGE_DEPENDS "libc6 (>= 2.34)") 22 | 23 | install(TARGETS control 24 | COMPONENT linapp 25 | RUNTIME DESTINATION "/usr/" 26 | LIBRARY DESTINATION "/usr/" 27 | PUBLIC_HEADER DESTINATION "/control/" 28 | DESTINATION "/usr/" 29 | ) 30 | 31 | install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/include/control DESTINATION include COMPONENT devel) 32 | 33 | SET(CPACK_GENERATOR "DEB") 34 | SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "Martin Schröder") 35 | INCLUDE(CPack) 36 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | .checkpatch.conf @mkschreder 2 | .clang-tidy @mkschreder 3 | /cmake/ @mkschreder 4 | /CMakeLists.txt @mkschreder 5 | /LICENSE @mkschreder 6 | /README.md @mkschreder 7 | /data/ @mkschreder 8 | /doc/ @mkschreder 9 | /include/ @mkschreder 10 | /integrity/ @mkschreder 11 | /octave/ @mkschreder 12 | /pictures/ @mkschreder 13 | /reports/ @mkschreder 14 | /scripts/ @mkschreder 15 | /src/ @mkschreder 16 | /tests/ @mkschreder 17 | /zephyr/ @mkschreder 18 | CODEOWNERS @mkschreder 19 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Daniel Mårtensson 4 | Copyright (c) 2022 Martin Schröder 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /cmake/clang-tidy.cmake: -------------------------------------------------------------------------------- 1 | find_program( 2 | CLANG_TIDY_EXE 3 | NAMES "clang-tidy-12" "clang-tidy" 4 | DOC "Path to clang-tidy executable") 5 | 6 | if(NOT CLANG_TIDY_EXE) 7 | message(FATAL_ERROR "clang-tidy not found") 8 | endif() 9 | 10 | function(clang_tidy_check TARGET) 11 | # Only check header files from the same current source dir 12 | get_filename_component(SOURCE_DIR_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) 13 | set(DO_CLANG_TIDY 14 | "${CLANG_TIDY_EXE}" "-extra-arg=-ferror-limit=0" 15 | "-p=${CMAKE_CURRENT_BUILD_DIR}/" "--header-filter=${SOURCE_DIR_NAME}/.*") 16 | 17 | set_target_properties(${TARGET} PROPERTIES C_CLANG_TIDY "${DO_CLANG_TIDY}") 18 | set_target_properties(${TARGET} PROPERTIES CXX_CLANG_TIDY "${DO_CLANG_TIDY}") 19 | endfunction() 20 | -------------------------------------------------------------------------------- /cmake/configure_tidy.cmake: -------------------------------------------------------------------------------- 1 | function(configure_tidy TARGET_NAME) 2 | if(CHECK_CLANG_TIDY) 3 | include(clang-tidy) 4 | clang_tidy_check(${TARGET_NAME}) 5 | endif() 6 | endfunction() 7 | -------------------------------------------------------------------------------- /doc/ai/a_star.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("a_star")} 8 | -------------------------------------------------------------------------------- /doc/ai/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | = Path Finding 8 | 9 | ${include("a_star.adoc", leveloffset="+0")} 10 | 11 | ${include("inpolygon.adoc", leveloffset="+0")} 12 | -------------------------------------------------------------------------------- /doc/ai/inpolygon.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("inpolygon")} 8 | -------------------------------------------------------------------------------- /doc/contents.toml.in: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "control" 3 | 4 | [asciidoc] 5 | src_dir = "${CMAKE_CURRENT_LIST_DIR}" 6 | 7 | [reference] 8 | type = "doxygen" 9 | dir = "${CMAKE_BINARY_DIR}/xml" 10 | -------------------------------------------------------------------------------- /doc/dynamics/c2d.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("c2d")} 8 | 9 | This is continuous to discrete transformation. It uses zero order hold (matrix 10 | exponential) method. 11 | -------------------------------------------------------------------------------- /doc/dynamics/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | = Discrete Dynamics 8 | 9 | This module focuses on functions that manipulate state-space and transfer 10 | function system dynamics. 11 | 12 | System identification is under a separate module that is not included here. 13 | 14 | ${include("c2d.adoc", leveloffset="+0")} 15 | 16 | ${include("kalman.adoc", leveloffset="+0")} 17 | 18 | ${include("lqi.adoc", leveloffset="+0")} 19 | 20 | ${include("mpc.adoc", leveloffset="+0")} 21 | 22 | ${include("mrac.adoc", leveloffset="+0")} 23 | 24 | ${include("pid.adoc", leveloffset="+0")} 25 | 26 | ${include("stability.adoc", leveloffset="+0")} 27 | 28 | ${include("theta2ss.adoc", leveloffset="+0")} 29 | -------------------------------------------------------------------------------- /doc/dynamics/kalman.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("kalman")} 8 | 9 | This is a static gain kalman filter algorithm. It is essentially a system 10 | propagation step with kalman gain applied to difference between observed state 11 | and predicted state. 12 | 13 | [stem] 14 | ++++ 15 | \hat{x} = Ax + Bu + Ky - KCx 16 | ++++ 17 | -------------------------------------------------------------------------------- /doc/dynamics/lqi.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("lqi")} 8 | 9 | This is a linear quadratic regulator with integral state. In your controller you 10 | would typically estimate error state instead of plant state so that you can 11 | drive the error output to zero. 12 | 13 | This means that you should compute the "measured" error as follows: 14 | 15 | [stem] 16 | ++++ 17 | e = -(r - y) 18 | ++++ 19 | 20 | The reason it is negative is because we are working with error state here. 21 | Suppose your plant is at zero (real state). You want to set the reference to 25. 22 | Your error at this point between measurement and reference is +25. But for the 23 | sake of control system design we should always set the reference to 0 because 24 | that's what our mathematical algorithms try to do - drive error to zero. So we 25 | simply assume that our plant is at -25 instead of 0 and so our error becomes 26 | -25. Hence the minus in the error calculation. 27 | -------------------------------------------------------------------------------- /doc/dynamics/mpc.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("mpc")} 8 | -------------------------------------------------------------------------------- /doc/dynamics/mrac.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("mrac")} 8 | -------------------------------------------------------------------------------- /doc/dynamics/pid.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("pid")} 8 | 9 | This is a pid controller with optional derivative filter integrated into the 10 | calculation. The derivative filter pole (parameter d) is a value of the discrete 11 | (z-domain) pole of the low pass filter that filters the derivative signal. 12 | 13 | The transfer function of this controller in discrete domain is: 14 | 15 | [stem] 16 | ++++ 17 | C = Kp + Kd * \frac{(1 - d)*(z - 1)}{z-d} + \frac{Ki}{z-1} 18 | ++++ 19 | -------------------------------------------------------------------------------- /doc/dynamics/stability.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("is_stable")} 8 | -------------------------------------------------------------------------------- /doc/dynamics/theta2ss.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("theta2ss")} 8 | -------------------------------------------------------------------------------- /doc/filter/filtfilt.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("filtfilt")} 8 | -------------------------------------------------------------------------------- /doc/filter/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | = Filtering 8 | 9 | ${include("filtfilt.adoc", leveloffset="+0")} 10 | 11 | ${include("mcs.adoc", leveloffset="+0")} 12 | 13 | ${include("sqr_ukf.adoc", leveloffset="+0")} 14 | -------------------------------------------------------------------------------- /doc/filter/mcs.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("mcs_collect")} 8 | 9 | ${insert("mcs_estimate")} 10 | 11 | ${insert("mcs_clean")} 12 | -------------------------------------------------------------------------------- /doc/filter/sqr_ukf.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("sqr_ukf")} 8 | -------------------------------------------------------------------------------- /doc/img/motion_profile_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/img/motion_profile_1.jpg -------------------------------------------------------------------------------- /doc/img/motion_profile_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/img/motion_profile_2.jpg -------------------------------------------------------------------------------- /doc/img/motion_profile_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/img/motion_profile_3.jpg -------------------------------------------------------------------------------- /doc/img/motion_profile_4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/img/motion_profile_4.jpg -------------------------------------------------------------------------------- /doc/linalg/add.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("add")} 8 | 9 | Element-wise addition of two matrices. 10 | -------------------------------------------------------------------------------- /doc/linalg/balance.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("balance")} 8 | 9 | The errors in calculating the eigenvalues of a square real matrix are 10 | proportional to the Frobenius norm of the matrix. Frobenius norm can be defined 11 | as a trace of square of the matrix (trace is sum of elements of the diagonal 12 | elements): 13 | 14 | [stem] 15 | ++++ 16 | ||A|| = \sqrt{trace(A*A)} 17 | ++++ 18 | 19 | The process of producing a matrix that is diagonally similar to the input matrix 20 | which reduces the matrix norm is called "balancing the matrix". 21 | 22 | This function transforms a matrix: 23 | 24 | 25 | [latexmath] 26 | ++++ 27 | \begin{bmatrix} -0.1 & 10.0 \\ -1.0 & 5.0 \end{bmatrix} 28 | ++++ 29 | 30 | Into a balanced matrix: 31 | 32 | 33 | [stem] 34 | ++++ 35 | \begin{bmatrix} -0.1 & 2.5 \\ -4.0 & 5.0 \end{bmatrix} 36 | ++++ 37 | -------------------------------------------------------------------------------- /doc/linalg/chol.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("chol")} 8 | 9 | This function performs lower triangular cholesky decomposition of a positive 10 | definite matrix A. The lower triangular decomposition is significantly faster 11 | for sparse matrices than upper triangular so this function implements only lower 12 | triangular decomposition. 13 | 14 | [stem] 15 | ++++ 16 | A = \begin{bmatrix} 5 & 11 \\ 11 & 25 \end{bmatrix} 17 | ++++ 18 | 19 | [stem] 20 | ++++ 21 | L = chol(A) = \begin{bmatrix} 2.23607 & 0.0 \\ 4.91935 & 0.89443 \end{bmatrix} 22 | ++++ 23 | 24 | [stem] 25 | ++++ 26 | A == L * L' 27 | ++++ 28 | -------------------------------------------------------------------------------- /doc/linalg/cholupdate.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("cholupdate")} 8 | 9 | Perform a Rank1 cholesky update or downdate on a lower triangular cholesky 10 | decomposition matrix L. 11 | 12 | Assume that: 13 | 14 | [stem] 15 | ++++ 16 | L = chol(A) 17 | ++++ 18 | 19 | When we want to compute: 20 | 21 | [stem] 22 | ++++ 23 | R = chol(A + x * x') 24 | ++++ 25 | 26 | Instead of computing the cholesky decomposition again, we can use cholupdate to 27 | get the new result without the cholesky decomposition: 28 | 29 | [stem] 30 | ++++ 31 | R = cholupdate(L, x) 32 | ++++ 33 | -------------------------------------------------------------------------------- /doc/linalg/det.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("det")} 8 | 9 | The determinant of a square matrix is a scalar value that is a function of the 10 | entries of the matrix. 11 | 12 | [stem] 13 | ++++ 14 | |A| = \begin{bmatrix} a & b \\ c & d \end{bmatrix} = ad-bc 15 | ++++ 16 | 17 | [stem] 18 | ++++ 19 | |A| = \begin{bmatrix} a & b & c \\ d & e & f \\ g & h & i \end{bmatrix} 20 | = a\,\begin{bmatrix} e & f \\ h & i \end{bmatrix} - 21 | b\,\begin{bmatrix} d & f \\ g & i \end{bmatrix} + 22 | c\,\begin{bmatrix} d & e \\ g & h \end{bmatrix} \\ 23 | = aei + bfg + cdh - ceg - bdi - afh. 24 | ++++ 25 | 26 | The determinant is non-zero if and only if the matrix is invertible and the 27 | linear map represented by the matdix is an isomorphism - i.e. an invertible 28 | function. 29 | 30 | The determinant of a product of matrices is the product of their determinants 31 | (the preceding property is a corollary of this one). 32 | -------------------------------------------------------------------------------- /doc/linalg/dlyap.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("dlyap")} 8 | 9 | The discrete time lyapunov equation is defined as: 10 | 11 | [stem] 12 | ++++ 13 | APA^{H} - P + Q = 0 14 | ++++ 15 | 16 | Where stem:[Q] is a hermitian matrix (a matrix that is equal to its conjugate 17 | transpose) and stem:[A^H] is the conjugate transpose of stem:[A]. 18 | -------------------------------------------------------------------------------- /doc/linalg/eig.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("eig")} 8 | 9 | Eigenvalues are a special set of scalars associated with a linear system of 10 | equations that are sometimes also referred to as characteristic roots, 11 | characteristic values, proper values, or latent roots. 12 | 13 | Equation stem:[(A − λI)v = 0] has a nonzero solution 'v' *if and only if* the 14 | determinant of the matrix 'A − λI' is zero. Therefore, the eigenvalues of 'A' 15 | are values of 'λ' that satisfy the equation: 16 | 17 | [stem] 18 | ++++ 19 | A-\lambda I| = 0 20 | ++++ 21 | 22 | This equation is called the characteristic equation or the 'secular equation' 23 | of A. This is useful when analyzing stability of a controlled system. 24 | -------------------------------------------------------------------------------- /doc/linalg/eig_sym.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("eig_sym")} 8 | 9 | Performs equivalent of following code in matlab: 10 | 11 | [source,matlab] 12 | -- 13 | [t, d] = eig(A) 14 | -- 15 | 16 | The result is a matrix of eigenvectors (t) and a diagonal matrix with 17 | eigenvalues (d). 18 | -------------------------------------------------------------------------------- /doc/linalg/expm.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("expm")} 8 | 9 | Compute matrix exponential stem:[e^A]. 10 | 11 | [source,matlab] 12 | -- 13 | E = expm(A) 14 | -- 15 | -------------------------------------------------------------------------------- /doc/linalg/hankel.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("hankel")} 8 | 9 | Compute hankel matrix from input matrix. 10 | 11 | A hankel matrix consists of input vector copied and shifted down the result 12 | matrix: 13 | 14 | [stem] 15 | ++++ 16 | \begin{bmatrix} a & b & c & d & e \\ b & c & d & e & f \\ c & d & e & f & g \\ d 17 | & e & f & g & h \\ e & f & g & h & i \end{bmatrix} 18 | ++++ 19 | 20 | A hankel matrix can also be half matrix: 21 | 22 | [stem] 23 | ++++ 24 | Input = \begin{bmatrix} a & b & c & d \end{bmatrix} 25 | ++++ 26 | 27 | Resulting hankel matrix: 28 | 29 | [stem] 30 | ++++ 31 | \begin{bmatrix} a & b \\ b & c \end{bmatrix} 32 | ++++ 33 | 34 | If the input vector has two or more rows then the resulting hankel matrix 35 | consists of blocks of that many rows: 36 | 37 | [stem] 38 | ++++ 39 | \begin{bmatrix} a & b & c & d & e \\ b & c & d & e & f \\ b & c & d & e & 0 \\ c 40 | & d & e & f & 0 \\ c & d & e & 0 & 0 \\ d & e & f & 0 & 0 \\ d & e & 0 & 0 & 0 41 | \\ e & f & 0 & 0 & 0 \\ e & 0 & 0 & 0 & 0 \\ f & 0 & 0 & 0 & 0 \end{bmatrix} 42 | ++++ 43 | -------------------------------------------------------------------------------- /doc/linalg/inv.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("inv")} 8 | -------------------------------------------------------------------------------- /doc/linalg/linsolve_chol.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("linsolve_chol")} 8 | 9 | This operation finds a solution 'x' in equation: 10 | 11 | [stem] 12 | ++++ 13 | Ax = b 14 | ++++ 15 | 16 | It uses cholesky decomposition to achieve this. 17 | -------------------------------------------------------------------------------- /doc/linalg/linsolve_gauss.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("linsolve_gauss")} 8 | 9 | This operation finds a solution 'x' in equation: 10 | 11 | [stem] 12 | ++++ 13 | Ax = b 14 | ++++ 15 | 16 | It uses gaussian elimination to achieve this. 17 | -------------------------------------------------------------------------------- /doc/linalg/linsolve_lower_triangular.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("linsolve_lower_triangular")} 8 | -------------------------------------------------------------------------------- /doc/linalg/linsolve_lup.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("linsolve_lup")} 8 | -------------------------------------------------------------------------------- /doc/linalg/linsolve_markov.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("linsolve_markov")} 8 | -------------------------------------------------------------------------------- /doc/linalg/linsolve_qr.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("linsolve_qr")} 8 | -------------------------------------------------------------------------------- /doc/linalg/linsolve_upper_triangular.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("linsolve_upper_triangular")} 8 | -------------------------------------------------------------------------------- /doc/linalg/lup.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("lup")} 8 | -------------------------------------------------------------------------------- /doc/linalg/mul.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("mul")} 8 | -------------------------------------------------------------------------------- /doc/linalg/nonlinsolve.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("nonlinsolve")} 8 | -------------------------------------------------------------------------------- /doc/linalg/norm.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("norm")} 8 | -------------------------------------------------------------------------------- /doc/linalg/pinv.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("pinv")} 8 | -------------------------------------------------------------------------------- /doc/linalg/qr.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("qr")} 8 | -------------------------------------------------------------------------------- /doc/linalg/sum.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("sum")} 8 | -------------------------------------------------------------------------------- /doc/linalg/svd_golub_reinsch.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("svd_golub_reinsch")} 8 | -------------------------------------------------------------------------------- /doc/linalg/svd_jacobi_one_sided.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("svd_jacobi_one_sided")} 8 | -------------------------------------------------------------------------------- /doc/linalg/tran.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("tran")} 8 | -------------------------------------------------------------------------------- /doc/misc/cat.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("cat")} 8 | -------------------------------------------------------------------------------- /doc/misc/constrain.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("constrain_float")} 8 | -------------------------------------------------------------------------------- /doc/misc/cut.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("cut")} 8 | -------------------------------------------------------------------------------- /doc/misc/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | = Misc 8 | 9 | ${include("cat.adoc", leveloffset="+0")} 10 | 11 | ${include("cut.adoc", leveloffset="+0")} 12 | 13 | ${include("insert.adoc", leveloffset="+0")} 14 | 15 | ${include("mean.adoc", leveloffset="+0")} 16 | 17 | ${include("print.adoc", leveloffset="+0")} 18 | 19 | ${include("randn.adoc", leveloffset="+0")} 20 | 21 | ${include("constrain.adoc", leveloffset="+0")} 22 | 23 | ${include("sign.adoc", leveloffset="+0")} 24 | 25 | ${include("stddev.adoc", leveloffset="+0")} 26 | -------------------------------------------------------------------------------- /doc/misc/insert.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("insert")} 8 | -------------------------------------------------------------------------------- /doc/misc/mean.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("mean")} 8 | -------------------------------------------------------------------------------- /doc/misc/print.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("print")} 8 | -------------------------------------------------------------------------------- /doc/misc/randn.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("randn")} 8 | -------------------------------------------------------------------------------- /doc/misc/sign.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("sign")} 8 | -------------------------------------------------------------------------------- /doc/misc/stddev.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("stddev")} 8 | -------------------------------------------------------------------------------- /doc/model/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | = Models 8 | 9 | ${include("dc_motor.adoc", leveloffset="+1")} 10 | -------------------------------------------------------------------------------- /doc/motion/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | = Profile Generation 8 | 9 | ${include("motion_profile.adoc", leveloffset="+0")} 10 | -------------------------------------------------------------------------------- /doc/motor/clarke.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("clarke")} 8 | 9 | Performs foward clarke transoformation. 10 | 11 | ${insert("inv_clarke")} 12 | 13 | Performs inverse clarke transformation. 14 | -------------------------------------------------------------------------------- /doc/motor/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | = Motor Control 8 | 9 | ${include("park.adoc", leveloffset="+1")} 10 | 11 | ${include("clarke.adoc", leveloffset="+1")} 12 | -------------------------------------------------------------------------------- /doc/motor/park.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("park")} 8 | 9 | Performs foward park transoformation. 10 | 11 | ${insert("inv_park")} 12 | 13 | Performs inverse park transformation. 14 | -------------------------------------------------------------------------------- /doc/optimization/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${include("linprog.adoc", leveloffset="+0")} 8 | -------------------------------------------------------------------------------- /doc/optimization/linprog.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("linprog")} 8 | -------------------------------------------------------------------------------- /doc/packages.toml.in: -------------------------------------------------------------------------------- 1 | [packages] 2 | 3 | [packages.control] 4 | type = "local" 5 | package_dir = "${CMAKE_BINARY_DIR}" 6 | -------------------------------------------------------------------------------- /doc/sysid/index.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${include("okid_era.adoc", leveloffset="+0")} 8 | 9 | ${include("rls.adoc", leveloffset="+0")} 10 | 11 | ${include("sqr_ukf_id.adoc", leveloffset="+0")} 12 | -------------------------------------------------------------------------------- /doc/sysid/okid_era.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("okid_era")} 8 | -------------------------------------------------------------------------------- /doc/sysid/rls.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("rls")} 8 | -------------------------------------------------------------------------------- /doc/sysid/sqr_ukf_id.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | // Copyright 2022 Martin Schröder 3 | // Consulting: https://swedishembedded.com/consulting 4 | // Simulation: https://swedishembedded.com/simulation 5 | // Training: https://swedishembedded.com/tag/training 6 | 7 | ${insert("sqr_ukf_id")} 8 | -------------------------------------------------------------------------------- /doc/theme/README.adoc: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: unknown 2 | = asciidoctor-pdf-tutorial-theme 3 | 4 | This is a custom theme for `asciidoctor-pdf` for producing technical 5 | documentation and tutorials with a modern look. 6 | 7 | == Status 8 | 9 | This repo is work in progress as not all layouts have been fully evaluated yet. 10 | 11 | == Usage 12 | 13 | Clone the repo to some directory, preferably within your document project e.g. `tutorial`. 14 | 15 | [source,shell] 16 | ---- 17 | git submodule add \ 18 | https://github.com/uroesch/asciidoctor-pdf-tutorial-theme \ 19 | tutorial 20 | ---- 21 | 22 | Run `asciidoctor-pdf` with the following additional options: 23 | 24 | [source,shell] 25 | ---- 26 | asciidoctor-pdf \ 27 | -a pdf-stylesdir=tutorial \ 28 | -a pdf-style=tutorial.yml \ 29 | -a pdf-fontsdir=tutorial/fonts \ 30 | -a iconsdir=tutorial/icons \ 31 | "book.adoc" 32 | ---- 33 | 34 | === Sample PDFs 35 | 36 | Sample PDFs generated from the `syntax.adoc`. 37 | 38 | [cols="1,1",options="header"] 39 | |=== 40 | 41 | | Doctype 42 | | Link 43 | 44 | | Book 45 | a| link:samples/book.pdf[Book sample PDF] 46 | 47 | | Article 48 | a| link:samples/article.pdf[Article sample PDF] 49 | 50 | | Man page 51 | a| link:samples/manpage.pdf[Man page sample PDF] 52 | 53 | |=== 54 | 55 | -------------------------------------------------------------------------------- /doc/theme/fonts/ClearSans-Bold.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/ClearSans-Bold.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/ClearSans-BoldItalic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/ClearSans-BoldItalic.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/ClearSans-Italic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/ClearSans-Italic.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/ClearSans-Light.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/ClearSans-Light.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/ClearSans-Medium.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/ClearSans-Medium.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/ClearSans-MediumItalic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/ClearSans-MediumItalic.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/ClearSans-Regular.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/ClearSans-Regular.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/ClearSans-Thin.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/ClearSans-Thin.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/Courier Prime Sans Bold Italic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/Courier Prime Sans Bold Italic.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/Courier Prime Sans Bold.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/Courier Prime Sans Bold.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/Courier Prime Sans Italic.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/Courier Prime Sans Italic.ttf -------------------------------------------------------------------------------- /doc/theme/fonts/Courier Prime Sans.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/fonts/Courier Prime Sans.ttf -------------------------------------------------------------------------------- /doc/theme/icons/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/icons/.gitignore -------------------------------------------------------------------------------- /doc/theme/icons/.placeholder: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/icons/.placeholder -------------------------------------------------------------------------------- /doc/theme/images/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/doc/theme/images/.gitignore -------------------------------------------------------------------------------- /include/control/ai.h: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #pragma once 11 | #include 12 | 13 | /** 14 | * \brief A* algorithm for path finding 15 | * \details 16 | * It find the optimal(at least try too) path from your source to your destination. 17 | * See working example how to use. 18 | * \param map Input map 19 | * \param path_x Output path x coordinates 20 | * \param path_y Output path y coordinates 21 | * \param x_start Starting x position 22 | * \param y_start Starting y position 23 | * \param x_stop End x position 24 | * \param y_stop End y position 25 | * \param height Height of the map 26 | * \param width Width of the map 27 | * \param norm_mode 1 or 2 (L1 or L2 norm) 28 | * \param steps Output variable which will contain number of steps taken 29 | **/ 30 | void a_star(const int *const map, int path_x[], int path_y[], int x_start, int y_start, int x_stop, 31 | int y_stop, int height, int width, uint8_t norm_mode, int *steps); 32 | /** 33 | * \brief Check if a point is inside a 2D polygon 34 | * \param x X coordinate 35 | * \param y Y coordinate 36 | * \param px X coordinates of the polygon 37 | * \param py Y coordinates of the polygon 38 | * \param p number of vertices 39 | * \retval 1 if inside 40 | * \retval 0 if outside 41 | **/ 42 | uint8_t inpolygon(float x, float y, const float *const px, const float *const py, uint8_t p); 43 | -------------------------------------------------------------------------------- /include/control/model.h: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /** 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/consulting 5 | * Simulation: https://swedishembedded.com/simulation 6 | * Training: https://swedishembedded.com/training 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "model/dc_motor.h" 12 | -------------------------------------------------------------------------------- /include/control/motor.h: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #pragma once 9 | 10 | /** 11 | * \brief power invariant park transform 12 | * \param dqz output dq frame 13 | * \param xyz input in xyz frame 14 | * \param angle electrical angle 15 | **/ 16 | void park(float *dqz, const float *const xyz, const float angle); 17 | 18 | /** 19 | * \brief power invariant inverse park transform 20 | * \param dqz input dq frame 21 | * \param xyz output in xyz frame 22 | * \param angle electrical angle 23 | **/ 24 | void inv_park(float *xyz, const float *const dqz, const float angle); 25 | 26 | /** 27 | * \brief clarke transform 28 | * \param xyz output in xyz frame 29 | * \param abc input phase voltages in rotating frame 30 | **/ 31 | void clarke(float *xyz, const float *const abc); 32 | 33 | /** 34 | * \brief clarke transform 35 | * \param xyz input in xyz frame 36 | * \param abc output phase voltages in rotating frame 37 | **/ 38 | void inv_clarke(float *abc, const float *const xyz); 39 | -------------------------------------------------------------------------------- /include/control/optimization.h: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | void linprog(float c[], float A[], float b[], float x[], uint8_t row_a, uint8_t column_a, 15 | uint8_t max_or_min, uint8_t iteration_limit); 16 | -------------------------------------------------------------------------------- /integrity/ai.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check ai 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/dynamics.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check dynamics 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/filter.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check filter 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/linalg.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check linalg 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/misc.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check misc 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/model.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check model 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/module.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | 10 | *** Variables *** 11 | ${ROOT_DIR} ${CURDIR}/../ 12 | 13 | *** Keywords *** 14 | 15 | Module structure check ${module} 16 | File Should Exist ${ROOT_DIR}/include/control/${module}.h 17 | File Should Exist ${ROOT_DIR}/tests/${module}/CMakeLists.txt 18 | Check Page Contains Text ${ROOT_DIR}/tests/CMakeLists.txt add_subdirectory(${module}) 19 | @{FILES} = List Files In Directory ${ROOT_DIR}/src/${module} *.c 20 | FOR ${FILE} IN @{FILES} 21 | ${PATH} ${NAME} Split Path ${FILE} 22 | ${SECTION} ${EXT} Split Extension ${NAME} 23 | File Should Exist ${ROOT_DIR}/doc/${module}/${SECTION}.adoc 24 | # Check that page is included in index 25 | Check Page Contains Text ${ROOT_DIR}/doc/${module}/index.adoc ${SECTION}.adoc 26 | # Check that test exists and that it is included in the build cmake file 27 | File Should Exist ${ROOT_DIR}/tests/${module}/${SECTION}.cpp 28 | Check Page Contains Text ${ROOT_DIR}/tests/${module}/CMakeLists.txt ${SECTION}.cpp 29 | # Check that file is in zephyr build as well 30 | Check Page Contains Text ${ROOT_DIR}/zephyr/CMakeLists.txt ../src/${module}/${SECTION}.c 31 | # Check that main doc index contains module reference 32 | Check Page Contains Text ${ROOT_DIR}/doc/index.adoc ${module}/index.adoc 33 | END 34 | -------------------------------------------------------------------------------- /integrity/motion.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check motion 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/motor.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check motor 18 | -------------------------------------------------------------------------------- /integrity/optimization.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check optimization 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/spdx.robot: -------------------------------------------------------------------------------- 1 | *** Settings *** 2 | Library OperatingSystem 3 | Library Process 4 | Library String 5 | 6 | *** Variables *** 7 | ${M_CONTROL_DIR} ${CURDIR}/../octave/control 8 | ${M_SYSID_DIR} ${CURDIR}/../octave/systemid 9 | ${C_INCLUDE_DIR} ${CURDIR}/../include/control 10 | ${SRC} ${CURDIR}/../src/ 11 | 12 | *** Test Cases *** 13 | 14 | All files have spdx license 15 | All *.m files 16 | All *.h files 17 | All *.c files 18 | All *.cpp files 19 | All *.yaml files 20 | All *.robot files 21 | All *.adoc files 22 | 23 | *** Keywords *** 24 | 25 | All ${wildcard} files 26 | @{files} = Git List Files ${wildcard} 27 | FOR ${FILE} IN @{FILES} 28 | ${length}= Get Length ${FILE} 29 | Run Keyword If ${length} File ${FILE} has spdx identifier 30 | END 31 | 32 | Git List Files ${wildcard} 33 | ${r} = Run Process git ls-files ${wildcard} 34 | @{list}= Split String ${r.stdout} \n 35 | [return] @{list} 36 | 37 | File ${FILE} has spdx identifier 38 | ${r} = Run Process grep SPDX ${FILE} 39 | Should Contain ${r.stdout} SPDX-License-Identifier: msg=SPDX identifier not found in ${FILE} 40 | -------------------------------------------------------------------------------- /integrity/sysid.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | *** Settings *** 7 | Library OperatingSystem 8 | Library ${CURDIR}/DocChecker.py 9 | Resource ${CURDIR}/module.robot 10 | 11 | *** Variables *** 12 | ${ROOT_DIR} ${CURDIR}/../ 13 | 14 | *** Test Cases *** 15 | 16 | Module structure is correct 17 | Module structure check sysid 18 | 19 | *** Keywords *** 20 | -------------------------------------------------------------------------------- /integrity/top-level.robot: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | # Check top level directories 7 | 8 | *** Settings *** 9 | Library OperatingSystem 10 | Library ${CURDIR}/DocChecker.py 11 | Resource ${CURDIR}/module.robot 12 | 13 | *** Variables *** 14 | ${ROOT_DIR} ${CURDIR}/../ 15 | 16 | *** Test Cases *** 17 | 18 | Modules have corresponding robot scripts 19 | @{MODULES} = List Directories In Directory ${ROOT_DIR}/src/ 20 | FOR ${module} IN @{MODULES} 21 | File Should Exist ${ROOT_DIR}/integrity/${module}.robot 22 | Check Page Contains Text ${ROOT_DIR}/integrity/${module}.robot Module structure check ${module} 23 | END 24 | 25 | *** Keywords *** 26 | -------------------------------------------------------------------------------- /octave/control/README.md: -------------------------------------------------------------------------------- 1 | Books which has been used are: 2 | * Grundläggande reglerteknik, 2002, edition 4, ISBN: 91-44-02416-9 - Bengt 3 | Lennartsson, Chalmers, Sverige 4 | * System Modeling and Identification, 2017, edition 2, ISBN: 0-13-482308-7, Can 5 | be purchased at http://kfsab.se/, - Rolf Johansson, Lund, Sverige 6 | 7 | -------------------------------------------------------------------------------- /octave/control/balreal.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates the balanced realization of a state space model 3 | % Input: sys 4 | % Example 1: model = balreal(sys) 5 | % Author: Daniel Mårtensson, Oktober 2017 6 | 7 | function [model] = balreal(varargin) 8 | % Check if there is any input 9 | if(isempty(varargin)) 10 | error ('Missing model') 11 | end 12 | 13 | % Get model type 14 | type = varargin{1}.type; 15 | % Check if there is a TF or SS model 16 | if(strcmp(type, 'SS' )) 17 | % Get sys 18 | sys = varargin{1}; 19 | % Get gramians 20 | Q = gram(sys, 'o'); 21 | P = gram(sys, 'c'); 22 | % Get hankel singular values 23 | E = diag(hsvd(sys)); 24 | 25 | % Begin 26 | Q1 = chol(Q); 27 | U = chol(eye(size(Q))); 28 | E1 = chol(E); 29 | 30 | T = inv(E1)*U'*Q1; 31 | 32 | % Get matrecies 33 | A = sys.A; 34 | B = sys.B; 35 | C = sys.C; 36 | D = sys.D; 37 | delay = sys.delay; 38 | sampleTime = sys.sampleTime; 39 | 40 | model = ss(delay, T*A*inv(T), T*B, C*inv(T), D); 41 | model.sampleTime = sampleTime; 42 | elseif(strcmp(type, 'TF' )) 43 | error('Transfer function model not supported. Convert to state space!') 44 | else 45 | error('This is not TF or SS'); 46 | end 47 | 48 | end 49 | -------------------------------------------------------------------------------- /octave/control/ctrb.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates the controllability matrix of a state space model 3 | % Input: sys, n(optinal) 4 | % Example 1: [Cs] = ctrb(sys) 5 | % Example 1: [Cs] = ctrb(sys, n) 6 | % Author: Daniel Mårtensson, Oktober 2017 7 | 8 | function [Cs] = ctrb(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing input') 12 | end 13 | 14 | % Get model type 15 | type = varargin{1}.type; 16 | % Check if there is a TF or SS model 17 | if(strcmp(type, 'SS' )) 18 | % Get SS 19 | sys = varargin{1}; 20 | 21 | % Check if we got variable n for minimal realization 22 | if(length(varargin) > 1) 23 | n = varargin{2}; 24 | else 25 | n = size(sys.A, 1); % Else, we only check the dimension of A 26 | end 27 | 28 | % Compute the controllability matrix now! 29 | Cs = []; 30 | for i = 0:(n-1) 31 | Cs = [Cs sys.A^i*sys.B]; 32 | end 33 | elseif(strcmp(type, 'TF' )) 34 | % Get TF 35 | error('Model must be a state space model'); 36 | else 37 | error('This is not TF or SS'); 38 | end 39 | end 40 | -------------------------------------------------------------------------------- /octave/control/d2c.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Transform discrete transfer function or state space model to continuous time 3 | % transfer function or state space model 4 | % Input: Gd, sysd 5 | % Example 1: G = d2c(Gd) 6 | % Example 2: sys = d2c(sysd) 7 | % Author: Daniel Mårtensson, September 2017 8 | 9 | function [model] = d2c(varargin) 10 | if(isempty(varargin{1})) 11 | error ('Missing model') 12 | end 13 | % State space 14 | if(strcmp(varargin{1}.type, 'SS' )) 15 | % Get info 16 | A = varargin{1}.A; 17 | B = varargin{1}.B; 18 | C = varargin{1}.C; 19 | D = varargin{1}.D; 20 | delay = varargin{1}.delay; 21 | sampleTime = varargin{1}.sampleTime; 22 | % Compute sizes 23 | a1 = size(A,2) + size(B,2) - size(A,1); 24 | b1 = size(A,2); 25 | a2 = size(A,2) + size(B,2) - size(B,1); 26 | b2 = size(B,2); 27 | % Compute square matrix 28 | M = [A B; zeros(a1, b1) eye(a2, b2)]; 29 | M = round(logm(M)*1/sampleTime*1/1e-6)*1e-6; % Very important to remove small numbers!! 30 | M(abs(M) < 1e-6) = 0; % Very importat to turn -0 to 0. 31 | A = M(1:size(A,1), 1:size(A,2)); 32 | B = M(1:size(B,1), (size(A,2) + 1):(size(A,2) + size(B,2))); 33 | % Return model 34 | model = ss(delay, A, B, C, D); 35 | % Don't forget to add sample time to zero again 36 | model.sampleTime = 0; 37 | elseif(strcmp(varargin{1}.type, 'TF' )) 38 | % Turn TF to SS 39 | sysd = tf2ss(varargin{1}, 'OCF'); 40 | % Call d2c2 41 | G = d2c(sysd); 42 | % SS to SS 43 | model = ss2tf(G); 44 | else 45 | error('No state space model or transfer function') 46 | end 47 | end 48 | -------------------------------------------------------------------------------- /octave/control/d2d.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Rediscrete transfer function or state space model 3 | % Input: Gd, sysd, sampleTime 4 | % Example 1: G = d2d(Gd, sampleTime) 5 | % Example 2: sys = d2d(sysd, sampleTime) 6 | % Author: Daniel Mårtensson, September 2017 7 | 8 | function [model] = d2d(varargin) 9 | if(isempty(varargin{1})) 10 | error ('Missing model') 11 | end 12 | if(isempty(varargin{2})) 13 | error ('Missing sampeltime') 14 | end 15 | % State space 16 | if(strcmp(varargin{1}.type, 'SS' )) 17 | % To continuous from discrete 18 | sys = d2c(varargin{1}); 19 | % To discrete from continuous 20 | model = c2d(sys, varargin{2}); 21 | elseif(strcmp(varargin{1}.type, 'TF' )) 22 | % To continuous from discrete 23 | G = d2c(varargin{1}); 24 | % To discrete from continuous 25 | model = c2d(G, varargin{2}); 26 | else 27 | error('No state space model or transfer function') 28 | end 29 | end 30 | -------------------------------------------------------------------------------- /octave/control/damp.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates zeros from transfer functions or state space models 3 | % Input: G, sys 4 | % Example 1: [Frequency, Damping, Poles, TimeConstant] = damp(G) 5 | % Example 2: [Frequency, Damping, Poles, TimeConstant] = damp(sys) 6 | % Author: Daniel Mårtensson, September 2017 7 | 8 | function [Frequency, Damping, Poles] = damp(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing input') 12 | end 13 | % Get model type 14 | type = varargin{1}.type; 15 | % Get sampleTime 16 | sampleTime = varargin{1}.sampleTime; 17 | % Check if there is a TF or SS model 18 | if(strcmp(type, 'SS' )) 19 | % Check if it's discrete 20 | if(sampleTime == 0) 21 | Poles = pole(varargin{1}) 22 | Frequency = abs(Poles) 23 | Damping = -cos(angle(Poles)) 24 | TimeConstant = (1./(Frequency.*Damping)) 25 | else 26 | % Discrete 27 | Poles = pole(varargin{1}) 28 | Frequency = abs(log(Poles)/sampleTime) 29 | Damping = -cos(angle(log(Poles))) 30 | TimeConstant = (1./(Frequency.*Damping)) 31 | end 32 | elseif(strcmp(type, 'TF' )) 33 | % Check if it's discrete 34 | if(sampleTime == 0) 35 | Poles = pole(varargin{1}) 36 | Frequency = abs(Poles) 37 | Damping = -cos(angle(Poles)) 38 | TimeConstant = (1./(Frequency.*Damping)) 39 | else 40 | % Discrete 41 | Poles = pole(varargin{1}) 42 | Frequency = abs(log(Poles)/sampleTime) 43 | Damping = -cos(angle(log(Poles))) 44 | TimeConstant = (1./(Frequency.*Damping)) 45 | end 46 | else 47 | error('This is not TF or SS'); 48 | end 49 | end 50 | -------------------------------------------------------------------------------- /octave/control/db2mag.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Convert decibel to magnitude 3 | % Input dB 4 | % Example 1: [mag] = db2mag(dB) 5 | % Author: Daniel Mårtensson, Oktober 2017 6 | function [mag] = db2mag (dB) 7 | mag = 10.^(dB/20); 8 | end 9 | -------------------------------------------------------------------------------- /octave/control/dcgain.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates the low frequency gain of a state space model or a transfer function 3 | % Input: sys, G 4 | % Example 1: dc = dcgain(sys) 5 | % Example 2: dc = dcgain(G) 6 | % Author: Daniel Mårtensson 2017 September 7 | 8 | function [dc] = dcgain(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing input') 12 | end 13 | 14 | % Get the type 15 | type = varargin{1}.type; 16 | % Check if there is a TF or SS model 17 | if(strcmp(type, 'SS' )) 18 | % Get necessary info 19 | A = varargin{1}.A; 20 | B = varargin{1}.B; 21 | C = varargin{1}.C; 22 | D = varargin{1}.D; 23 | dc = C*inv(-A)*B + D; 24 | elseif(strcmp(type, 'TF' )) 25 | % Get necessary info 26 | for i = 1:size(varargin{1},1) 27 | for j = 1:size(varargin{1},2) 28 | % Get necessary info 29 | G = varargin{1}(i, j); 30 | % Get the static gain 31 | dc(i, j) = G.num(length(G.num))/G.den(length(G.den)); 32 | % If divided by zero - Is not a number 33 | if isnan(dc(i, j)) 34 | disp(sprintf('Divided my zero - dcgain (%i, %i) set to 0', i, j)) 35 | dc(i, j) = 0; 36 | end 37 | end 38 | end 39 | else 40 | error('This is not TF or SS'); 41 | end 42 | end 43 | -------------------------------------------------------------------------------- /octave/control/gensig.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates a signal of sin, square or pulse 3 | % Input: type, amp, Tf, Ts 4 | % Example 1: [u, t] = gensig(type, amp, Tf, Ts) 5 | % Author: Daniel Mårtensson, Oktober 2017 6 | 7 | function [u, t] = gensig(varargin) 8 | % Check if there is any input 9 | if(isempty(varargin)) 10 | error('Missing input') 11 | end 12 | % Get type 13 | if(length(varargin) > 0) 14 | type = varargin{1}; 15 | else 16 | error('Missing type') 17 | end 18 | % Get the amplitude 19 | if(length(varargin) > 1) 20 | amp = varargin{2}; 21 | else 22 | amp = 1; 23 | end 24 | % Get the Tf 25 | if(length(varargin) > 2) 26 | Tf = varargin{3}; 27 | else 28 | Tf = 1; 29 | end 30 | % Get time 31 | if(length(varargin) > 3) 32 | Ts = varargin{4}; 33 | else 34 | Ts = 10; 35 | end 36 | 37 | switch type 38 | case 'sin' 39 | t = linspace(0,Ts, 1000); 40 | u = amp*sin(Tf*t + Ts); 41 | case 'square' 42 | t = linspace(0,Ts, 1000); 43 | u = []; 44 | n = 1; 45 | if(Tf == 1) 46 | Tf = 2; 47 | end 48 | for i = 1:length(t) 49 | if(i >= length(t)/(Tf)*n) 50 | u(i) = amp; 51 | if(i >= length(t)/(Tf)*(n + 1)) 52 | n = n + 2; 53 | end 54 | else 55 | u(i) = 0; 56 | end 57 | end 58 | case 'pulse' 59 | t = linspace(0,Ts, 1000); 60 | u = []; 61 | n = 1; 62 | if(Tf == 1) 63 | Tf = 2; 64 | end 65 | for i = 1:length(t) 66 | if(i >= length(t)/(Tf)*n) 67 | u(i) = amp; 68 | n = n + 1; 69 | else 70 | u(i) = 0; 71 | end 72 | end 73 | u(end) = 0; % Remove the last peak 74 | end 75 | end 76 | -------------------------------------------------------------------------------- /octave/control/gram.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates the gramian a state space model, where opt = 'c' or opt = 'o' 3 | % Input: sys, opt 4 | % Example 1: X = gram(sys, opt) 5 | % Author: Daniel Mårtensson, Oktober 2017 6 | 7 | function [X] = gram(varargin) 8 | % Check if there is any input 9 | if(isempty(varargin)) 10 | error ('Missing model') 11 | end 12 | 13 | if(length(varargin) >= 2) 14 | opt = varargin{2}; 15 | else 16 | error('Missing option: ´c´ or ´o´'); 17 | end 18 | 19 | % Get model type 20 | type = varargin{1}.type; 21 | % Check if there is a TF or SS model 22 | if(strcmp(type, 'SS' )) 23 | % Get sys 24 | sys = varargin{1}; 25 | % Get sample time 26 | sampleTime = varargin{1}.sampleTime; 27 | 28 | if(sampleTime > 0) 29 | % Discrete way 30 | A = sys.A; 31 | B = sys.B; 32 | C = sys.C; 33 | 34 | if(strcmp(opt, 'c')) 35 | X = lyap(A, B*B', 'd'); 36 | elseif(strcmp(opt, 'o')) 37 | X = lyap(A', C'*C, 'd'); 38 | else 39 | disp(sprintf('Unknown option: %s', opt)); 40 | end 41 | else 42 | % Continous way 43 | A = sys.A; 44 | B = sys.B; 45 | C = sys.C; 46 | 47 | if(strcmp(opt, 'c')) 48 | X = lyap(A, B*B'); 49 | elseif(strcmp(opt, 'o')) 50 | X = lyap(A', C'*C); 51 | else 52 | disp(sprintf('Unknown option: %s', opt)); 53 | end 54 | end 55 | elseif(strcmp(type, 'TF' )) 56 | disp('Only state space model are allowed') 57 | else 58 | error('This is not TF or SS'); 59 | end 60 | end 61 | -------------------------------------------------------------------------------- /octave/control/hsvd.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Get the hankel singular values of a state space model 3 | % Input: SS, p(optional) 4 | % Example 1: hsv = hsvd(sys) 5 | % Example 1: hsv = hsvd(sys, p) % p = 'plot' 6 | % Author: Daniel Mårtensson, Oktober 2017 7 | 8 | function [hsv] = hsvd(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing model') 12 | end 13 | 14 | if(length(varargin) >= 2) 15 | p = varargin{2}; 16 | else 17 | p = 'n'; 18 | end 19 | 20 | % Get model type 21 | type = varargin{1}.type; 22 | % Check if there is a TF or SS model 23 | if(strcmp(type, 'SS' )) 24 | % Get sys 25 | sys = varargin{1}; 26 | % Get gramians 27 | P = gram(sys, 'c'); 28 | Q = gram(sys, 'o'); 29 | % Get hankel singular values 30 | hsv = sqrt(eig(P*Q)); 31 | % Plot them too! 32 | if(strcmp(p,'plot')) 33 | bar(hsv) 34 | title('Hankel Singular Values (State Contributions)') 35 | xlabel('State') 36 | ylabel('State Energy') 37 | legend('Stable modes') 38 | end 39 | elseif(strcmp(type, 'TF' )) 40 | error('Model must be a state space model') 41 | else 42 | error('This is not TF or SS'); 43 | end 44 | end 45 | -------------------------------------------------------------------------------- /octave/control/impulse.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Do a impulse response of a transfer function or a state space model 3 | % Input: G, sys, t(optional) 4 | % Example 1: [y,t,x] = impulse(G, t) 5 | % Example 2: [y,t,x] = impulse(G) 6 | % Example 3: [y,t,x] = impulse(sys, t) 7 | % Author: Daniel Mårtensson, September 2017 8 | 9 | function [y,t,X] = impulse(varargin) 10 | % Check if there is some input arguments or it's not a model 11 | if(isempty(varargin{1})) 12 | error ('Missing input') 13 | end 14 | 15 | % Check if there is a model 16 | if(strcmp(varargin{1}.type,'SS')) 17 | % Get time 18 | if(length(varargin) >= 2) 19 | t = varargin{2}; 20 | else 21 | disp('Time assumed to be 10 seconds'); 22 | t = 10; 23 | end 24 | 25 | % Get sample time to compute the new time vector 26 | sampleTime = varargin{1}.sampleTime; 27 | if(sampleTime > 0) 28 | t = 0:sampleTime:t; 29 | else 30 | t = 0:0.01:t; % Sample time assumed to be 0.01 31 | end 32 | 33 | % Multiple signals...or not! 34 | u = zeros(size(varargin{1}.B, 2), length(t)); % Creates 1 1 1 1 1 1 1 35 | u(:,1) = ones(size(u,1), 1); 36 | x0 = zeros(size(varargin{1}.A, 1), 1); % Assume x0 = [0; 0; 0; ..... ; 0] 37 | 38 | % Call lsim! 39 | [y,t,X] = lsim(varargin{1}, u, t, x0); 40 | elseif(strcmp(varargin{1}.type,'TF')) 41 | % TF to SS 42 | sys = tf2ss(varargin{1}, 'OCF'); 43 | 44 | % Get time 45 | if(length(varargin) >= 2) 46 | t = varargin{2}; 47 | else 48 | disp('Time assumed to be 10 seconds'); 49 | t = 10; 50 | end 51 | 52 | % Call impulse 53 | [y,t,X] = impulse(sys,t); 54 | else 55 | error('Not a state space model or a transfer function') 56 | end 57 | end 58 | -------------------------------------------------------------------------------- /octave/control/initial.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Simulate a transfer function or state space model with initial state vector x0 3 | % and time constant t, not time vector 4 | % Input: sys, t, x0 5 | % Example 1: [y,t,x] = initial(sys, t, x0) 6 | % Author: Daniel Mårtensson, September 2017 7 | 8 | function [y,t,X] = initial(varargin) 9 | % Check if there is some input arguments or it's not a model 10 | if(isempty(varargin{1})) 11 | error ('Missing input') 12 | end 13 | 14 | % Check if there is a model 15 | if(strcmp(varargin{1}.type,'SS')) 16 | 17 | % Get time 18 | if(length(varargin) >= 2) 19 | t = varargin{2}; 20 | else 21 | disp('Time assumed to be 10 seconds'); 22 | t = 10; 23 | end 24 | 25 | % Get sample time to compute the new time vector 26 | sampleTime = varargin{1}.sampleTime; 27 | if(sampleTime > 0) 28 | t = 0:sampleTime:t; 29 | else 30 | t = 0:0.01:t; % Sample time assumed to be 0.01 31 | end 32 | 33 | % Multiple signals...or not! 34 | u = zeros(size(varargin{1}.B, 2), length(t)); % Creates 0 0 0 0 0 0 0 35 | 36 | % Get initial conditions 37 | if(length(varargin) >= 3) 38 | x0 = varargin{3}; 39 | x0 = x0(:); % Turn them into a vector 40 | if(size(varargin{1}.A, 1) ~= size(x0, 1)) 41 | error('The initial conditions vector has not the same row length as matrix A') 42 | end 43 | else 44 | error('Missing initial conditions'); 45 | end 46 | 47 | % Call lsim! 48 | [y,t,X] = lsim(varargin{1}, u, t, x0); 49 | elseif(strcmp(varargin{1}.type,'TF')) 50 | error('Only for state space models') 51 | else 52 | error('Not a state space model or a transfer function') 53 | end 54 | end 55 | -------------------------------------------------------------------------------- /octave/control/loop.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Create the Complementary sensitivity, sensitivity of an open loop transfer function L 3 | % Input: L 4 | % Example 1: [S, T] = loop(L) 5 | % Author: Daniel Mårtensson, Oktober 2017 6 | function [S, T] = loop(varargin) 7 | % Check if there is any input 8 | if(isempty(varargin)) 9 | error ('Missing transfer function') 10 | end 11 | 12 | % Get model type 13 | type = varargin{1}.type; 14 | % Check if there is a TF or SS model 15 | if(strcmp(type, 'SS' )) 16 | error('Only loop transfer functions'); 17 | elseif(strcmp(type, 'TF' )) 18 | % Get open loop transfer function 19 | L = varargin{1}; 20 | % Create S 21 | N = tf(1,1); % Create a constant only for sensitivity function 22 | S = feedback(L, N); 23 | % Create T 24 | T = feedback(L, L); 25 | else 26 | error('This is not TF or SS'); 27 | end 28 | end 29 | -------------------------------------------------------------------------------- /octave/control/lqe.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Compute the LQE kalman gain matrix with the weighing matricies Q and R and state space model 3 | % Input: SS, Q, R 4 | % Example 1: [K] = lqe(sys, Q, R) 5 | % Author: Daniel Mårtensson, October 2017 6 | 7 | function [K] = lqe(varargin) 8 | % Check if there is any input 9 | if(isempty(varargin)) 10 | error ('Missing model') 11 | end 12 | 13 | % Get Q R 14 | if(length(varargin) >= 2) 15 | Q = varargin{2}; 16 | R = varargin{3}; 17 | else 18 | error('Missing Q or R'); 19 | end 20 | 21 | % Get model type 22 | type = varargin{1}.type; 23 | % Check if there is a TF or SS model 24 | if(strcmp(type, 'SS' )) 25 | % Get model 26 | sys = varargin{1}; 27 | % Change matrecies due to the Kalman Duality! 28 | sys.A = sys.A'; % Transpose 29 | sys.B = sys.C'; % Transpose 30 | K = (lqr(sys, Q, R))'; 31 | elseif(strcmp(type, 'TF' )) 32 | disp('Only state space models only') 33 | else 34 | error('This is not TF or SS'); 35 | end 36 | end 37 | -------------------------------------------------------------------------------- /octave/control/lqr.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Compute the LQR gain matrix control law L with the weighing matricies Q and R 3 | % and state space model 4 | % Input: sys, Q, R 5 | % Example 1: [L] = lqr(sys, Q, R) 6 | % Author: Daniel Mårtensson, October 2017 7 | 8 | function [L] = lqr(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing model') 12 | end 13 | 14 | % Get Q R 15 | if(length(varargin) >= 2) 16 | Q = varargin{2}; 17 | R = varargin{3}; 18 | else 19 | error('Missing Q or R'); 20 | end 21 | 22 | % Get model type 23 | type = varargin{1}.type; 24 | % Check if there is a TF or SS model 25 | if(strcmp(type, 'SS' )) 26 | % Get model 27 | sys = varargin{1}; 28 | % Solve the Algebraic Riccati Equation 29 | X = are(sys, Q, R); 30 | % Return the control law L 31 | if(sys.sampleTime > 0) 32 | L = inv(sys.B'*X*sys.B + R)*(sys.B'*X*sys.A); 33 | else 34 | L = inv(R)*sys.B'*X; 35 | end 36 | elseif(strcmp(type, 'TF' )) 37 | error('Model must be a state space model') 38 | else 39 | error('This is not TF or SS'); 40 | end 41 | end 42 | -------------------------------------------------------------------------------- /octave/control/lyap.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Compute solution X of the Lyapunov equation 3 | % Input: A, Q, opt(optinal) 4 | % Example 1: [X] = lyap(A, B*B'); % t = time contious as default 5 | % Example 2: [X] = lyap(A, B*B', 'd'); % d = discrete 6 | % Author: Daniel Mårtensson, Oktober 2017 7 | 8 | function [X] = lyap(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing A, Q') 12 | end 13 | 14 | A = varargin{1}; 15 | Q = varargin{2}; 16 | 17 | if(length(varargin) <= 2) 18 | opt = 't'; 19 | else 20 | opt = varargin{3}; 21 | end 22 | 23 | if(strcmp(opt, 'd')) 24 | p = kron(conj(A), A); 25 | K = eye(size(p)) - p; 26 | X = K\Q(:); 27 | X = reshape(X, size(A)); 28 | elseif(strcmp(opt, 't')) 29 | K = kron(eye(size(A)), A) + kron(conj(A), eye(size(A))); 30 | X = K\-Q(:); 31 | X = reshape(X, size(A)); 32 | else 33 | disp(sprintf('Unknown option: %s', opt)) 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /octave/control/mag2db.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Convert magnitude to decibel 3 | % Input mag 4 | % Example 1: [dB] = mag2db(mag) 5 | % Author: Daniel Mårtensson 2017, Oktober 6 | function [dB] = mag2db(mag) 7 | mag(mag < 0) = NaN; 8 | dB = 20*log10(mag); 9 | end 10 | -------------------------------------------------------------------------------- /octave/control/modred.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates the balanced recuded state space model of a balanced state space model 3 | % Remove states with the tolerance of Hankel singular values 4 | % Input: SS, tol 5 | % Example 1: model = modred(sys, tol) 6 | % Author: Daniel Mårtensson, 2017 October 7 | 8 | function [model] = modred(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing balanced state space model') 12 | end 13 | 14 | if(length(varargin) >= 2) 15 | tol = varargin{2}; 16 | else 17 | error('Missing tolerance for the hankel signular values'); 18 | end 19 | 20 | % Get model type 21 | type = varargin{1}.type; 22 | % Check if there is a TF or SS model 23 | if(strcmp(type, 'SS' )) 24 | % Get sys 25 | sys = varargin{1}; 26 | A = sys.A; 27 | B = sys.B; 28 | C = sys.C; 29 | D = sys.D; 30 | sampleTime = sys.sampleTime; 31 | delay = sys.delay; 32 | 33 | % Get hankel singular values 34 | hsv = diag(hsvd(sys)); 35 | hsv = hsv(hsv >= tol); 36 | n = size(hsv, 1); % Dimension states 37 | A = A(1:n, 1:n); 38 | B = B(1:n, :); 39 | C = C(:,1:n); 40 | 41 | % Create the state space model 42 | model = ss(delay, A, B, C, D); 43 | model.sampleTime = sampleTime; 44 | elseif(strcmp(type, 'TF' )) 45 | error('Model must be a state space model') 46 | else 47 | error('This is not TF or SS'); 48 | end 49 | end 50 | -------------------------------------------------------------------------------- /octave/control/nlsim.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/octave/control/nlsim.m -------------------------------------------------------------------------------- /octave/control/obsv.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates the observability matrix of a state space model 3 | % Input: sys, n(optinal) 4 | % Example 1: Or = obsv(sys) 5 | % Example 2: Or = obsv(sys, n) 6 | % Author: Daniel Mårtensson, 2017 Oktober 7 | 8 | function [Or] = obsv(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing input') 12 | end 13 | 14 | % Get model type 15 | type = varargin{1}.type; 16 | % Check if there is a TF or SS model 17 | if(strcmp(type, 'SS' )) 18 | % Get SS 19 | sys = varargin{1}; 20 | 21 | % If we specify n, that means we are using minreal.m 22 | if(length(varargin) > 1) 23 | n = varargin{2}; 24 | else 25 | n = size(sys.A, 1); % Else, we only check the dimension of A 26 | end 27 | 28 | % Compute the observability matrix now! 29 | Or = []; 30 | for i = 0:(n-1) 31 | Or = [Or; sys.C*sys.A^i]; 32 | end 33 | 34 | elseif(strcmp(type, 'TF' )) 35 | % Get TF 36 | error('Model must be a state space model'); 37 | else 38 | error('This is not TF or SS'); 39 | end 40 | end 41 | -------------------------------------------------------------------------------- /octave/control/pid.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates the parallel PID controller as a transfer function 3 | % Input: Kp, Ti(Integrator, Optinal), Td(Derivative, Optional), Tf(Low pass 4 | % filter, Optinal), Ts(Sample time, Optinal) 5 | % Example 1: [Gpid] = pid(Kp, Ti, Td, Tf, Ts) 6 | % Author: Daniel Mårtensson, Oktober 2017 7 | 8 | function [Gpid] = pid(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing parameters') 12 | end 13 | 14 | % Input the parameters 15 | if(length(varargin) >= 1) 16 | Kp = tf(varargin{1}, 1); 17 | Gpid = Kp; 18 | else 19 | error('Need to have at least Kp'); 20 | end 21 | 22 | % Integrator 23 | if(length(varargin) >= 2) 24 | Ki = tf(varargin{2}, [1 0]); 25 | Gpid = parallel(Kp, Ki); 26 | else 27 | Ti = 0; 28 | end 29 | 30 | % Derivative 31 | if(length(varargin) >= 3) 32 | Td = varargin{3}; 33 | else 34 | Td = 0; 35 | end 36 | 37 | % Low pass filter for derivative 38 | if(length(varargin) >= 4) 39 | Tf = varargin{4}; 40 | Kd = tf([Td 0], [Tf 1]); 41 | else 42 | Tf = 0; 43 | Kd = tf([Td 0], [1]); 44 | end 45 | 46 | % Sampling time 47 | if(length(varargin) >= 5) 48 | Ts = varargin{5}; 49 | else 50 | Ts = 0; 51 | end 52 | 53 | % Build the PID 54 | % Check if derivative was 0 55 | if Td > 0 56 | Gpid = parallel(Gpid, Kd); 57 | end 58 | 59 | % Else - Just return Gpid as it is 60 | 61 | % Convert to discrete if needed 62 | if(Ts > 0) 63 | Gpid = c2d(Gpid, Ts); 64 | end 65 | 66 | end 67 | -------------------------------------------------------------------------------- /octave/control/pipd.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates the serial PID controller as a transfer function 3 | % Input: Kp, Ti(optinal), Td(optional), b(optinal), Ts(optinal) 4 | % Example 1: [Gpipd] = pipd(Kp, Ti, Td, b, Ts) 5 | % Author: Daniel Mårtensson, Oktober 2017 6 | 7 | function [Gpipd] = pipd(varargin) 8 | % Check if there is any input 9 | if(isempty(varargin)) 10 | error ('Missing parameters') 11 | end 12 | 13 | % Input the parameters 14 | if(length(varargin) >= 1) 15 | Kp = varargin{1}; 16 | else 17 | error('Need to have at least Kp'); 18 | end 19 | 20 | % Integrator 21 | if(length(varargin) >= 2) 22 | Ti = varargin{2}; 23 | else 24 | Ti = 0; 25 | end 26 | 27 | % Derivative 28 | if(length(varargin) >= 3) 29 | Td = varargin{3}; 30 | else 31 | Td = 0; 32 | end 33 | 34 | % High pass filter 35 | if(length(varargin) >= 4) 36 | b = varargin{4}; 37 | else 38 | b = 0; 39 | end 40 | 41 | % Sampling time 42 | if(length(varargin) >= 5) 43 | Ts = varargin{5}; 44 | else 45 | Ts = 0; 46 | end 47 | 48 | % Build the PIPD 49 | Gpipd = tf([Kp*Ti*Td Kp*(Ti+Td) Kp],[Ti*Td/b Ti 0]); 50 | 51 | % Convert to discrete if needed 52 | if(Ts > 0) 53 | Gpipd = c2d(Gpipd, Ts); 54 | end 55 | end 56 | -------------------------------------------------------------------------------- /octave/control/pole.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generates poles from transfer functions or state space models 3 | % Input: TF or SS 4 | % Example 1: p = pole(G) 5 | % Example 2: p = pole(sys) 6 | % Author: Daniel Mårtensson 2017 September 7 | 8 | function [p] = pole(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing input') 12 | end 13 | 14 | % Get model type 15 | type = varargin{1}.type; 16 | % Check if there is a TF or SS model 17 | if(strcmp(type, 'SS' )) 18 | % Get poles 19 | A = varargin{1}.A; 20 | p = eig(A); % Eigenvalues 21 | elseif(strcmp(type, 'TF' )) 22 | % Get poles 23 | G = varargin{1}; 24 | p = roots(G.den); 25 | else 26 | error('This is not TF or SS'); 27 | end 28 | end 29 | -------------------------------------------------------------------------------- /octave/control/pzmap.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Plot poles and zeros of a transfer function or of a state space model 3 | % Input: G, sys 4 | % Example 1: [p, z] = pzmap(G) 5 | % Example 2: [p, z] = pzmap(sys) 6 | % Author: Daniel Mårtensson, 2017 September 7 | 8 | function [p, z] = pzmap(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing input') 12 | end 13 | 14 | % Get model type 15 | type = varargin{1}.type; 16 | % Check if there is a TF or SS model 17 | if(strcmp(type, 'SS' )) 18 | % Get poles 19 | p = pole(varargin{1}) 20 | z = tzero(varargin{1}) 21 | plot(real(p), imag(p), 'o', real(z), imag(z), 'x'); 22 | grid on 23 | title('Pole-Zero Map') 24 | xlabel('Real axis'); 25 | ylabel('Imaginary axis'); 26 | 27 | % If we have no zeros - It can happen! 28 | if isempty(z) 29 | legend('poles'); 30 | else 31 | legend('poles', 'zeros'); 32 | end 33 | elseif(strcmp(type, 'TF' )) 34 | % Get G 35 | G = varargin{1}; 36 | % Get poles and zeros 37 | z = roots(G.num); 38 | p = roots(G.den); 39 | plot(real(p), imag(p), 'o', real(z), imag(z), 'x'); 40 | grid on 41 | title('Pole-Zero Map') 42 | xlabel('Real axis'); 43 | ylabel('Imaginary axis'); 44 | 45 | % If we have no zeros - It can happen! 46 | if isempty(z) 47 | legend('poles'); 48 | else 49 | legend('poles', 'zeros'); 50 | end 51 | else 52 | error('This is not TF or SS'); 53 | end 54 | end 55 | -------------------------------------------------------------------------------- /octave/control/ramp.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Ramp a state space model or a transfer function 3 | % Input: sys, G, t(optinal) 4 | % Example 1: [y,t,x] = ramp(G, t) 5 | % Example 2: [y,t,x] = ramp(G) 6 | % Example 3: [y,t,x] = ramp(sys, t) 7 | % Author: Daniel Mårtensson, 2017 September 8 | 9 | function [y,t,X] = ramp(varargin) 10 | % Check if there is some input arguments or it's not a model 11 | if(isempty(varargin{1})) 12 | error ('Missing input') 13 | end 14 | 15 | % Check if there is a model 16 | if(strcmp(varargin{1}.type,'SS')) 17 | 18 | % Get time 19 | if(length(varargin) >= 2) 20 | t = varargin{2}; 21 | else 22 | disp('Time assumed to be 10 seconds'); 23 | t = 10; 24 | end 25 | 26 | % Get sample time to compute the new time vector 27 | sampleTime = varargin{1}.sampleTime; 28 | if(sampleTime > 0) 29 | t = 0:sampleTime:t; 30 | else 31 | t = 0:0.01:t; % Sample time assumed to be 0.01 32 | end 33 | 34 | % Multiple signals...or not! 35 | u = linspace(0,1, length(t)); 36 | u = repmat(u, size(varargin{1}.B, 2), 1); % Creates 0 -> 1 37 | x0 = zeros(size(varargin{1}.A, 1), 1); % Assume x0 = [0; 0; 0; ..... ; 0] 38 | % Call lsim! 39 | [y,t,X] = lsim(varargin{1}, u, t, x0); 40 | elseif(strcmp(varargin{1}.type,'TF')) 41 | % TF to SS 42 | sys = tf2ss(varargin{1}, 'OCF'); 43 | 44 | % Get time 45 | if(length(varargin) >= 2) 46 | t = varargin{2}; 47 | else 48 | disp('Time assumed to be 10 seconds'); 49 | t = 10; 50 | end 51 | 52 | % Call ramp 53 | [y,t,X] = ramp(sys,t); 54 | else 55 | error('Not a state space model or a transfer function') 56 | end 57 | end 58 | -------------------------------------------------------------------------------- /octave/control/referencegain.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Computes the reference gain Kr and give back a state space model with 3 | % including the reference gain 4 | % Input: sys 5 | % Example 1: [sys_kr, Kr] = referencegain(sys) 6 | % Author: Daniel Mårtensson, September 2018 7 | 8 | function [sys_kr, Kr] = referencegain(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error ('Missing input') 12 | end 13 | % Get model type 14 | type = varargin{1}.type; 15 | % Check if there is a TF or SS model 16 | if(strcmp(type, 'SS' )) 17 | % Get A, B, C, D matrecies 18 | sys = varargin{1}; 19 | A = sys.A; 20 | B = sys.B; 21 | C = sys.C; 22 | D = sys.D; 23 | delay = sys.delay; 24 | sampleTime = sys.sampleTime; 25 | 26 | if sampleTime > 0 27 | Kr = 1./(C*inv(eye(size(A)) - A)*B); 28 | else 29 | Kr = 1./(C*inv(-A)*B); 30 | end 31 | % Now create B matrix with precompensator factor - For better tracking 32 | B = B*Kr; 33 | 34 | sys_kr = ss(delay, A, B, C, D); 35 | sys_kr.sampleTime = sampleTime; 36 | 37 | elseif(strcmp(type, 'TF' )) 38 | disp('Only state space models') 39 | else 40 | error('This is not TF or SS'); 41 | end 42 | end 43 | -------------------------------------------------------------------------------- /octave/control/sgrid.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Author: Daniel Mårtensson, 2017 Oktober 3 | 4 | function sgrid(z, wn) 5 | % Do the circle 6 | ang=0:0.01:2*pi; 7 | xp=wn*cos(ang); 8 | yp=wn*sin(ang); 9 | 10 | % Do the slope lines 11 | y1 = [0 wn*sqrt(1-z^2)]; 12 | y2 = [0 -wn*sqrt(1-z^2)]; 13 | x1 = [0 -z*wn]; 14 | x2 = [0 -z*wn]; 15 | 16 | % Plot now 17 | hold on 18 | plot(xp, yp,'.-', x1, y1, '.-', x2, y2, '.-') 19 | end 20 | -------------------------------------------------------------------------------- /octave/control/step.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Simulate continuous time model of a state space model or transfer function 3 | % Input: G, sys, t(optional) 4 | % Example 1: [y,t,x] = step(G, t) 5 | % Example 2: [y,t,x] = step(G) 6 | % Example 3: [y,t,x] = step(sys, t) 7 | % Author: Daniel Mårtensson, 2017 September 8 | 9 | function [y,t,X] = step(varargin) 10 | % Check if there is some input arguments or it's not a model 11 | if(isempty(varargin{1})) 12 | error ('Missing input') 13 | end 14 | 15 | % Check if there is a model 16 | if(strcmp(varargin{1}.type,'SS')) 17 | 18 | % Get time 19 | if(length(varargin) >= 2) 20 | t = varargin{2}; 21 | else 22 | disp('Time assumed to be 10 seconds'); 23 | t = 10; 24 | end 25 | 26 | % Get sample time to compute the new time vector 27 | sampleTime = varargin{1}.sampleTime; 28 | if(sampleTime > 0) 29 | t = 0:sampleTime:t; 30 | else 31 | t = 0:0.01:t; % Sample time assumed to be 0.01 32 | end 33 | 34 | % Multiple signals...or not! 35 | u = ones(size(varargin{1}.B, 2), length(t)); % Creates 1 1 1 1 1 1 1 36 | x0 = zeros(size(varargin{1}.A, 1), 1); % Assume x0 = [0; 0; 0; ..... ; 0] 37 | 38 | % Call lsim! 39 | [y,t,X] = lsim(varargin{1}, u, t, x0); 40 | elseif(strcmp(varargin{1}.type,'TF')) 41 | % TF to SS 42 | sys = tf2ss(varargin{1}, 'OCF'); 43 | 44 | % Get time 45 | if(length(varargin) >= 2) 46 | t = varargin{2}; 47 | else 48 | disp('Time assumed to be 10 seconds'); 49 | t = 10; 50 | end 51 | 52 | % Call step 53 | [y,t,X] = step(sys,t); 54 | else 55 | error('Not a state space model or a transfer function') 56 | end 57 | end 58 | -------------------------------------------------------------------------------- /octave/control/zpk.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Generate a transfer function from vector of zero, vector of poles and vectior of gain 3 | % Input: z, p , k, delay(optional) 4 | % Example 1: G = zpk(z, p, k) 5 | % Example 2: G = zpk(z, p, k, delay) 6 | % Author: Daniel Mårtensson, 2017 September 7 | 8 | 9 | function [G] = zpk(varargin) 10 | % Check if there is any input 11 | if(isempty(varargin)) 12 | error ('Missing input') 13 | end 14 | 15 | % Get zeros 16 | if(length(varargin) < 1) 17 | error('Missing zeros') 18 | else 19 | z = varargin{1}; 20 | end 21 | % Get poles 22 | if(length(varargin) < 2) 23 | error('Missing zeros') 24 | else 25 | p = varargin{2}; 26 | end 27 | 28 | % Get gain 29 | if(length(varargin) < 3) 30 | error('Missing gain') 31 | else 32 | k = varargin{3}; 33 | end 34 | 35 | % Check if values are cells 36 | if(~iscell(z)) 37 | z = {z}; 38 | end 39 | 40 | if(~iscell(p)) 41 | p = {p}; 42 | end 43 | 44 | % The gain k is never cell 45 | 46 | % Get numerator and denomerator 47 | num = cellfun (@(zer, gain) real (gain * poly (zer)), z, num2cell(k), 'uniformoutput', false); 48 | den = cellfun (@(pol) real (poly (pol)), p, 'uniformoutput', false); 49 | 50 | % Get delay 51 | if(length(varargin) >= 4) 52 | delay = varargin{4}; 53 | G = tf(round(cell2mat(num)*1/1e-6)*1e-6, round(cell2mat(den)*1/1e-6)*1e-6, delay); % Tar bort alla 000000 på slutet 54 | else 55 | G = tf(cell2mat(num), cell2mat(den)); 56 | end 57 | 58 | 59 | end 60 | -------------------------------------------------------------------------------- /octave/systemid/filtfilt2.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Filtfilt2 - A filter with no or less phase shifting. Very simple to use. 3 | % Input: y(noisy signal), t(time signal), K(optional filter factor) 4 | % Output: y(clean signal) 5 | % Example 1: [y] = filtfilt2(y, t); 6 | % Example 2: [y] = filtfilt2(y, t, K); 7 | % Author: Daniel Mårtensson, April 2020 8 | % Update: 27 April 2020 for MIMO signals 9 | % Update: 29 April 2020 for Euler method 10 | 11 | function [y] = filtfilt2(varargin) 12 | % Check if there is any input 13 | if(isempty(varargin)) 14 | error('Missing imputs') 15 | end 16 | 17 | % Get input 18 | if(length(varargin) >= 1) 19 | y = varargin{1}; 20 | else 21 | error('Missing input y') 22 | end 23 | 24 | % Get time 25 | if(length(varargin) >= 2) 26 | t = varargin{2}; 27 | else 28 | error('Missing input t') 29 | end 30 | 31 | % Get filter factor 32 | if(length(varargin) >= 3) 33 | K = varargin{3}; 34 | else 35 | K = 0.1; 36 | end 37 | 38 | % Find size of y 39 | m = size(y, 1); 40 | n = size(y, 2); 41 | 42 | for i = 1:m 43 | % Simulate the noisy signal 44 | y1 = simulation(K, y(i, 1:n), t); 45 | 46 | % Flip 47 | y2 = flip(y1); 48 | 49 | % Run the simulation again 50 | y3 = simulation(K, y2, t); 51 | 52 | % Flip - Done 53 | y4 = flip(y3); 54 | 55 | % Save it 56 | y(i, 1:n) = y4; 57 | end 58 | 59 | end 60 | 61 | % Euler method for simple ODE 62 | function [y] = simulation(K, y, t); 63 | h = t(2)-t(1); % Time step 64 | x = y(1); % Initial state 65 | for i = 1:length(t) 66 | x = x + h*(-1/K*x + 1/K*y(i)); 67 | y(i) = x; % Save 68 | end 69 | end 70 | -------------------------------------------------------------------------------- /octave/systemid/okid.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/octave/systemid/okid.m -------------------------------------------------------------------------------- /octave/systemid/rpca.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/octave/systemid/rpca.m -------------------------------------------------------------------------------- /octave/systemid/spa.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Plot bode spectral analysis plot using Fast Fourier Transform 3 | % Input: y(frequency output), t(time) 4 | % Output: amp(amplitude), wout(frequencies) 5 | % Example 1: [amp, wout] = spa(y, t); 6 | % Author: Daniel Mårtensson, November 2017 7 | 8 | function [amp, wout] = spa(varargin) 9 | % Check if there is any input 10 | if(isempty(varargin)) 11 | error('Missing imputs') 12 | end 13 | 14 | 15 | % Get output 16 | if(length(varargin) >= 1) 17 | y = varargin{1}; 18 | else 19 | error('Missing output'); 20 | end 21 | 22 | % Get time 23 | if(length(varargin) >= 2) 24 | t = varargin{2}; 25 | else 26 | error('Missing time'); 27 | end 28 | 29 | Ts = t(2)-t(1); % Sampling time 30 | Fs = 1/Ts; % Sampling rate 31 | % Do FFT 32 | F = fft(y, Fs); 33 | Y = abs(F); 34 | 35 | % Compute the frequencies 36 | freq = (0:Fs-1)(1:end/2 +1); 37 | % Compute the amplitudes 38 | Y = Y(1:end/2)/length(Y)*2; 39 | 40 | plot(freq, Y) 41 | ylabel('Amplitude') 42 | xlabel('Frequency [Hz]') 43 | 44 | % Return values 45 | amp = 20*log10(Y); 46 | wout = freq; 47 | 48 | end 49 | -------------------------------------------------------------------------------- /pictures/AppliedSystemIdentification.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/AppliedSystemIdentification.jpeg -------------------------------------------------------------------------------- /pictures/ERADC_Result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/ERADC_Result.png -------------------------------------------------------------------------------- /pictures/ERADC_System.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/ERADC_System.png -------------------------------------------------------------------------------- /pictures/FILTFILT2_Result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/FILTFILT2_Result.png -------------------------------------------------------------------------------- /pictures/FestoBench.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/FestoBench.jpg -------------------------------------------------------------------------------- /pictures/ICA_After.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/ICA_After.png -------------------------------------------------------------------------------- /pictures/ICA_Before.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/ICA_Before.png -------------------------------------------------------------------------------- /pictures/ICA_Mixed_Signals.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/ICA_Mixed_Signals.png -------------------------------------------------------------------------------- /pictures/IDBODE_Result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/IDBODE_Result.png -------------------------------------------------------------------------------- /pictures/IDBODE_System.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/IDBODE_System.png -------------------------------------------------------------------------------- /pictures/Markering_024.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Markering_024.png -------------------------------------------------------------------------------- /pictures/OCID_Result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/OCID_Result.png -------------------------------------------------------------------------------- /pictures/OCID_System.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/OCID_System.png -------------------------------------------------------------------------------- /pictures/OKID_Result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/OKID_Result.png -------------------------------------------------------------------------------- /pictures/OKID_System.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/OKID_System.jpg -------------------------------------------------------------------------------- /pictures/PLC system.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/PLC system.jpg -------------------------------------------------------------------------------- /pictures/RLS_Result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/RLS_Result.png -------------------------------------------------------------------------------- /pictures/RLS_System.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/RLS_System.jpg -------------------------------------------------------------------------------- /pictures/RPCA_Bob.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/RPCA_Bob.png -------------------------------------------------------------------------------- /pictures/RolfJohanssonsBok.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/RolfJohanssonsBok.jpg -------------------------------------------------------------------------------- /pictures/SINDY_Result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SINDY_Result.png -------------------------------------------------------------------------------- /pictures/SINDY_Result_multivariable.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SINDY_Result_multivariable.png -------------------------------------------------------------------------------- /pictures/SPA_Result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SPA_Result.png -------------------------------------------------------------------------------- /pictures/SR-UKF/sr-ukf-paper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SR-UKF/sr-ukf-paper.pdf -------------------------------------------------------------------------------- /pictures/SR-UKF/sr-ukf-parameter-estimation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SR-UKF/sr-ukf-parameter-estimation.png -------------------------------------------------------------------------------- /pictures/SR-UKF/sr-ukf-state-estimation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SR-UKF/sr-ukf-state-estimation.png -------------------------------------------------------------------------------- /pictures/SR_UKF_parameter_estimation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SR_UKF_parameter_estimation.png -------------------------------------------------------------------------------- /pictures/SR_UKF_state_estimation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SR_UKF_state_estimation.png -------------------------------------------------------------------------------- /pictures/SVM_c_header.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SVM_c_header.png -------------------------------------------------------------------------------- /pictures/SVM_c_source.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SVM_c_source.png -------------------------------------------------------------------------------- /pictures/SVM_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SVM_plot.png -------------------------------------------------------------------------------- /pictures/SVM_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/SVM_results.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-00-55.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-00-55.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-01-11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-01-11.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-04-32.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-04-32.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-04-59.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-04-59.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-06-29.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-06-29.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-06-41.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-06-41.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-07-36.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-07-36.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-08-23.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-08-23.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-09-19.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-09-19.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-10-03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-10-03.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-11-30.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-11-30.png -------------------------------------------------------------------------------- /pictures/Skärmbild från 2017-11-09 00-12-02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/Skärmbild från 2017-11-09 00-12-02.png -------------------------------------------------------------------------------- /pictures/cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/pictures/cover.png -------------------------------------------------------------------------------- /pictures/examples.md: -------------------------------------------------------------------------------- 1 | # Examples 2 | This folder are all the examples such as pictures etc. 3 | -------------------------------------------------------------------------------- /reports/Applied System Identification - Lecture note 1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/Applied System Identification - Lecture note 1.pdf -------------------------------------------------------------------------------- /reports/Applied System Identification - Lecture note 2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/Applied System Identification - Lecture note 2.pdf -------------------------------------------------------------------------------- /reports/Applied System Identification - Lecture note 3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/Applied System Identification - Lecture note 3.pdf -------------------------------------------------------------------------------- /reports/ERADC.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/ERADC.pdf -------------------------------------------------------------------------------- /reports/ICA.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/ICA.pdf -------------------------------------------------------------------------------- /reports/OCID.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/OCID.pdf -------------------------------------------------------------------------------- /reports/OKID.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/OKID.pdf -------------------------------------------------------------------------------- /reports/RLS.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/RLS.pdf -------------------------------------------------------------------------------- /reports/SINDY.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/SINDY.pdf -------------------------------------------------------------------------------- /reports/SSFD.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/SSFD.pdf -------------------------------------------------------------------------------- /reports/sr-ukf-paper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/reports/sr-ukf-paper.pdf -------------------------------------------------------------------------------- /results.txt: -------------------------------------------------------------------------------- 1 | float A[ADIM * ADIM] = { 2 | 0.895595,0.377345, 3 | -0.377345,0.518249}; 4 | 5 | float B[ADIM * RDIM] = { 6 | 0.208811, 7 | 0.754690}; 8 | 9 | float C[YDIM * ADIM] = { 10 | 1.000000,0.000000}; 11 | 12 | float D[YDIM * RDIM] = { 13 | 0.000000}; 14 | 15 | float K[ADIM * YDIM] = { 16 | 0.580062, 17 | -0.223907}; 18 | 19 | float L[RDIM * ADIM] = { 20 | 1.567665,0.851028}; 21 | 22 | float Li[RDIM] = { 23 | 0.501345}; 24 | 25 | -------------------------------------------------------------------------------- /scripts/build: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # SPDX-License-Identifier: Apache-2.0 3 | # Copyright (c) 2022 Martin Schröder 4 | # Consulting: https://swedishembedded.com/go 5 | # Training: https://swedishembedded.com/tag/training 6 | 7 | [[ -d build ]] && rm -rf build 8 | 9 | set -e 10 | 11 | mkdir -p build 12 | cd build 13 | cmake \ 14 | -DCHECK_CLANG_TIDY=ON \ 15 | .. 16 | make -j8 17 | make docs | grep "warning:" && { 18 | echo "There were documentation errors" 19 | echo "Rerun 'make docs' again and check it" 20 | exit 1 21 | } 22 | cpack 23 | cd .. 24 | -------------------------------------------------------------------------------- /scripts/checkpatch/typedefsfile: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/scripts/checkpatch/typedefsfile -------------------------------------------------------------------------------- /scripts/ci/check-file-sorted: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # SPDX-License-Identifier: Apache-2.0 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | set -e 7 | 8 | if [[ ! "$1" ]]; then 9 | echo "Usage: $0 " 10 | exit 1 11 | fi 12 | 13 | # check that file is sorted 14 | LC_ALL=C sort --check $1 || { 15 | echo "File $1 is not sorted"; 16 | echo "Run: LC_ALL=C sort -u -o $1 $1" 17 | exit 1 18 | } 19 | 20 | -------------------------------------------------------------------------------- /scripts/ci/check-links: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # SPDX-License-Identifier: Apache-2.0 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | ROOT="$(realpath $(dirname $BASH_SOURCE)/../..)" 7 | 8 | set -e 9 | 10 | ERROR=0 11 | for FILE in `git ls-files \ 12 | "*CMakeLists.txt" \ 13 | "*.cmake" \ 14 | "*Kconfig" \ 15 | "*.robot" \ 16 | "*prj.conf" \ 17 | "*.c" \ 18 | "*.h" \ 19 | "*.cs" \ 20 | "*.cpp" \ 21 | "*.yaml" \ 22 | "*.dts" \ 23 | "*.overlay" \ 24 | "*.rst" 25 | `; do 26 | grep "Copyright " ${FILE} > /dev/null || { 27 | echo "$FILE: no copyright information" 28 | ERROR=1 29 | } 30 | grep "SPDX-License-Identifier: " ${FILE} > /dev/null || { 31 | echo "$FILE: no SPDX license" 32 | ERROR=1 33 | } 34 | grep "Consulting: " ${FILE} > /dev/null || { 35 | echo "$FILE: no consulting offer" 36 | ERROR=1 37 | } 38 | grep "Training: " ${FILE} > /dev/null || { 39 | echo "$FILE: no training offer" 40 | ERROR=1 41 | } 42 | done 43 | 44 | exit $ERROR 45 | -------------------------------------------------------------------------------- /scripts/ci/twister_ignore.txt: -------------------------------------------------------------------------------- 1 | # Add one pattern per line. 2 | # 3 | # The patterns listed in this file will be compared with the list of files 4 | # changed in a patch series (Pull Request) and if all files in the pull request 5 | # are matched, then twister will not do a full run and optionally will only 6 | # run on changed tests or boards. 7 | # 8 | .buildkite/daily.yml 9 | .gitlint 10 | .checkpatch.conf 11 | .clang-format 12 | .codecov.yml 13 | .editorconfig 14 | .gitattributes 15 | .gitignore 16 | .mailmap 17 | .uncrustify.cfg 18 | CODEOWNERS 19 | MAINTAINERS.yml 20 | LICENSE 21 | Makefile 22 | tests/* 23 | samples/* 24 | boards/*/*/* 25 | arch/xtensa/* 26 | arch/x86/* 27 | arch/posix/* 28 | arch/arc/* 29 | arch/sparc/* 30 | arch/arm/* 31 | arch/nios2/* 32 | arch/riscv/* 33 | include/arch/xtensa/* 34 | include/arch/x86/* 35 | include/arch/posix/* 36 | include/arch/arc/* 37 | include/arch/sparc/* 38 | include/arch/arm/* 39 | include/arch/nios2/* 40 | include/arch/riscv/* 41 | doc/* 42 | # GH action have no impact on code 43 | .github/* 44 | *.rst 45 | *.md 46 | # if we change this file or associated script, it should not trigger a full 47 | # twister. 48 | scripts/ci/twister_ignore.txt 49 | scripts/ci/what_changed.py 50 | scripts/ci/version_mgr.py 51 | scripts/requirements* 52 | scripts/checkpatch/* 53 | scripts/checkpatch.pl 54 | scripts/ci/pylintrc 55 | scripts/footprint/* 56 | -------------------------------------------------------------------------------- /scripts/coverage: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | lcov -c -i -d . -o coverage.info 4 | lcov -c -d . -o coverage.info 5 | genhtml -o coverage coverage.info 6 | -------------------------------------------------------------------------------- /scripts/init: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # SPDX-License-Identifier: Apache-2.0 3 | # Copyright (c) 2022 Martin Schröder 4 | # Consulting: https://swedishembedded.com/go 5 | # Training: https://swedishembedded.com/tag/training 6 | 7 | apt-get update && 8 | apt-get install -qy libgtest-dev clang-tidy 9 | pip3 install asciidoxy 10 | -------------------------------------------------------------------------------- /scripts/sort-codeowners: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # SPDX-License-Identifier: Apache-2.0 3 | # Copyright (c) 2022 Martin Schröder 4 | # Consulting: https://swedishembedded.com/go 5 | # Training: https://swedishembedded.com/tag/training 6 | 7 | ROOT="$(realpath $(dirname $BASH_SOURCE)/..)" 8 | 9 | LC_ALL=C sort -u -o \ 10 | ${ROOT}/CODEOWNERS \ 11 | ${ROOT}/CODEOWNERS 12 | -------------------------------------------------------------------------------- /scripts/test: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # SPDX-License-Identifier: Apache-2.0 3 | # Copyright (c) 2022 Martin Schröder 4 | # Consulting: https://swedishembedded.com/go 5 | # Training: https://swedishembedded.com/tag/training 6 | 7 | [[ -d build ]] && rm -rf build 8 | 9 | mkdir -p build 10 | cd build 11 | cmake \ 12 | -D"CMAKE_CXX_FLAGS=-g -O2 -w -fprofile-arcs -ftest-coverage" \ 13 | -D"CMAKE_C_FLAGS=-g -O2 -w -fprofile-arcs -ftest-coverage" .. 14 | make -j4 15 | make test 16 | ctest -T Coverage 17 | ../scripts/coverage 18 | cd .. 19 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | file(GLOB_RECURSE SOURCES ./**/*.c) 7 | 8 | add_library(control STATIC ${SOURCES}) 9 | 10 | target_compile_options(control PRIVATE -Wall -Wextra -Werror -pedantic) 11 | 12 | target_include_directories(control PUBLIC "${CMAKE_SOURCE_DIR}/include") 13 | 14 | configure_tidy(control) 15 | -------------------------------------------------------------------------------- /src/ai/a_star.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/src/ai/a_star.c -------------------------------------------------------------------------------- /src/ai/inpolygon.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/ai.h" 11 | #include "control/misc.h" 12 | 13 | #include 14 | 15 | uint8_t inpolygon(float x, float y, const float *const px, const float *const py, uint8_t p) 16 | { 17 | /* Get the max y, min y, max x, min y */ 18 | float max_y = py[0]; 19 | float max_x = px[0]; 20 | float min_y = py[0]; 21 | float min_x = px[0]; 22 | 23 | for (uint8_t i = 0; i < p; i++) { 24 | max_y = fmaxf(py[i], max_y); 25 | max_x = fmaxf(px[i], max_x); 26 | min_y = fminf(py[i], min_y); 27 | min_x = fminf(px[i], min_x); 28 | } 29 | 30 | /* Check if we are outside the polygon */ 31 | if (y < min_y || y > max_y || x < min_x || x > max_x) { 32 | return 0; 33 | } 34 | 35 | /* Do a better check */ 36 | uint8_t ok = 0; 37 | uint8_t i = 0; 38 | uint8_t j = p - 1; 39 | 40 | while (i < p) { 41 | if (((px[i] > x) != (px[j] > x)) && 42 | (y < (py[j] - py[i]) * (x - px[i]) / (px[j] - px[i]) + py[i])) { 43 | ok = !ok; 44 | } 45 | j = i; 46 | i++; 47 | } 48 | return ok; 49 | } 50 | -------------------------------------------------------------------------------- /src/dynamics/kalman.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | #include "control/dynamics.h" 12 | 13 | void kalman(float *xout, const float *const A, const float *x, const float *const B, 14 | const float *const u, const float *const K, const float *const y, const float *const C, 15 | uint8_t ADIM, uint8_t YDIM, uint8_t RDIM) 16 | { 17 | float Ax[ADIM * 1]; 18 | float Bu[ADIM * 1]; 19 | float Cx[YDIM * 1]; 20 | float KCx[ADIM * 1]; 21 | float Ky[ADIM * 1]; 22 | 23 | mul(Ax, A, x, ADIM, ADIM, ADIM, 1); 24 | mul(Bu, B, u, ADIM, RDIM, RDIM, 1); 25 | mul(Cx, C, x, YDIM, ADIM, ADIM, 1); 26 | mul(KCx, K, Cx, ADIM, YDIM, YDIM, 1); 27 | mul(Ky, K, y, ADIM, YDIM, YDIM, 1); 28 | 29 | // Estimate new discrete state 30 | // x = Ax + Bu + K * (y - Cx) 31 | for (uint8_t i = 0; i < ADIM; i++) { 32 | xout[i] = Ax[i] + Bu[i] + Ky[i] - KCx[i]; 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /src/dynamics/mpc.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/src/dynamics/mpc.c -------------------------------------------------------------------------------- /src/dynamics/mrac.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/src/dynamics/mrac.c -------------------------------------------------------------------------------- /src/dynamics/pid.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Copyright 2022 Martin Schröder 3 | % Consulting: https://swedishembedded.com/go 4 | % Training: https://swedishembedded.com/tag/training 5 | 6 | % Design of the pid controller 7 | dir = fileparts(mfilename('fullpath')); 8 | path(strcat(dir, "/../../octave/"), path); 9 | 10 | pkg load symbolic 11 | 12 | syms Kp Ki Kd d Ts z 13 | 14 | G = Kp + Kd * ((1 - d)*(z - 1))/(z-d) + Ki / (z-1); 15 | G = simplify(expand(G)); 16 | G = sym_tf(G, Ts); 17 | [N, D] = numden(G) 18 | ccode(G) 19 | -------------------------------------------------------------------------------- /src/dynamics/stability.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | #include "control/dynamics.h" 12 | 13 | #include 14 | 15 | bool is_stable(const float *const A, uint8_t ADIM) 16 | { 17 | float wr[ADIM]; // Real eigenvalues 18 | float wi[ADIM]; // Imaginary eigenvalues 19 | bool stable = true; // Assume that the system is stable 20 | 21 | eig(A, wr, wi, ADIM); 22 | for (uint8_t i = 0; i < ADIM; i++) { 23 | float abs_value = sqrtf(wr[i] * wr[i] + wi[i] * wi[i]); 24 | 25 | if (abs_value > 1) { 26 | stable = false; 27 | } 28 | } 29 | return stable; 30 | } 31 | -------------------------------------------------------------------------------- /src/filter/filtfilt.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/filter.h" 11 | 12 | #include 13 | #include 14 | 15 | // Euler method for simple ODE - Low pass filter 16 | static void simulation(float K, float *y, const float *const t, uint16_t l) 17 | { 18 | float h = t[1] - t[0]; // Time step 19 | float x = y[0]; // Initial state 20 | 21 | for (uint16_t i = 0; i < l; i++) { 22 | x = x + h * (-1 / K * x + 1 / K * y[i]); 23 | y[i] = x; // Save 24 | } 25 | } 26 | 27 | // Flip signal 28 | static void flip(float y[], uint16_t l) 29 | { 30 | for (uint16_t i = 0; i < l / 2; i++) { 31 | float temp = y[i]; 32 | 33 | y[i] = y[l - 1 - i]; 34 | y[l - 1 - i] = temp; 35 | } 36 | } 37 | 38 | void filtfilt(float *y_out, const float *const y, const float *const t, uint16_t l, float K) 39 | { 40 | memcpy(y_out, y, sizeof(float) * l); 41 | 42 | // Simulate 43 | simulation(K, y_out, t, l); 44 | 45 | // Flip y 46 | flip(y_out, l); 47 | 48 | // Run the simulation again 49 | simulation(K, y_out, t, l); 50 | 51 | // Flip again - Done 52 | flip(y_out, l); 53 | } 54 | -------------------------------------------------------------------------------- /src/linalg/add.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | 14 | /* 15 | * C = A + B 16 | */ 17 | void add(float *C, const float *const A, const float *const B, uint16_t row, uint16_t column) 18 | { 19 | float Cr[row * column]; 20 | 21 | for (uint16_t i = 0; i < row; i++) { 22 | for (uint16_t j = 0; j < column; j++) { 23 | Cr[i * column + j] = A[i * column + j] + B[i * column + j]; 24 | } 25 | } 26 | memcpy(C, Cr, sizeof(float) * row * column); 27 | } 28 | -------------------------------------------------------------------------------- /src/linalg/balance.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | 14 | /* 15 | * Balance a real matrix 16 | * A [m*n] 17 | */ 18 | void balance(float A[], uint16_t row) 19 | { 20 | uint16_t i, j, last = 0; 21 | float s, r, g, f, c, sqrdx; 22 | 23 | sqrdx = 4.0f; 24 | while (last == 0) { 25 | last = 1; 26 | for (i = 0; i < row; i++) { 27 | r = c = 0.0f; 28 | for (j = 0; j < row; j++) 29 | if (j != i) { 30 | c += fabsf(*(A + row * j + i)); 31 | r += fabsf(*(A + row * i + j)); 32 | } 33 | if (c != 0.0 && r != 0.0) { 34 | g = r / 2.0f; 35 | f = 1.0f; 36 | s = c + r; 37 | while (c < g) { 38 | f *= 2.0f; 39 | c *= sqrdx; 40 | } 41 | g = r * 2.0f; 42 | while (c > g) { 43 | f /= 2.0f; 44 | c /= sqrdx; 45 | } 46 | if ((c + r) / f < 0.95f * s) { 47 | last = 0; 48 | g = 1.0f / f; 49 | for (j = 0; j < row; j++) 50 | *(A + row * i + j) *= g; 51 | for (j = 0; j < row; j++) 52 | *(A + row * j + i) *= f; 53 | } 54 | } 55 | } 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /src/linalg/chol.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | /* 17 | * Create A = L*L^T 18 | * A need to be symmetric positive definite 19 | * A [m*n] 20 | * L [m*n] 21 | * n == m 22 | */ 23 | void chol(const float *const A, float *L, uint16_t row) 24 | { 25 | float s; 26 | uint16_t i, j, k; 27 | 28 | memset(L, 0, row * row * sizeof(float)); 29 | for (i = 0; i < row; i++) 30 | for (j = 0; j <= i; j++) { 31 | s = 0; 32 | for (k = 0; k < j; k++) 33 | s += L[row * i + k] * L[row * j + k]; 34 | 35 | // We cannot divide with zero 36 | if (L[row * j + j] == 0) { 37 | L[row * j + j] = FLT_EPSILON; // Same as eps command in MATLAB 38 | } 39 | L[row * i + j] = (i == j) ? sqrtf(A[row * i + i] - s) : 40 | (1.0f / L[row * j + j] * (A[row * i + j] - s)); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /src/linalg/det.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include 11 | 12 | /* 13 | * Computes the determinant of square matrix A 14 | * A [m*n] 15 | * n == m 16 | * Return: Determinant value, or 0 for singular matrix 17 | */ 18 | float det(const float *const A, uint16_t row) 19 | { 20 | float determinant = 1.0f; 21 | float LU[row * row]; 22 | uint8_t P[row]; 23 | 24 | if (lup(A, LU, P, row) != 0) { 25 | // LU decomposition failed 26 | // matrix is singular 27 | return 0; 28 | } 29 | 30 | for (uint16_t i = 0; i < row; ++i) 31 | determinant *= LU[row * P[i] + i]; 32 | 33 | int j = 0; 34 | 35 | for (uint16_t i = 0; i < row; ++i) 36 | if (P[i] != i) 37 | ++j; 38 | 39 | if (j && (j - 1) % 2 == 1) 40 | determinant = -determinant; 41 | 42 | return determinant; 43 | } 44 | -------------------------------------------------------------------------------- /src/linalg/hankel.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | #include 14 | 15 | /* 16 | * Create hankel matrix of vector V. Step is just the shift. Normaly set this to 0. 17 | * V [m*n] 18 | * H [m*n] 19 | * shift >= 0 // Set this to 0 if you want a normal hankel matrix 20 | */ 21 | int hankel(const float *const V, float *H, uint16_t row_v, uint16_t column_v, uint16_t row_h, 22 | uint16_t column_h, uint16_t shift) 23 | { 24 | // row_h need to be divided with row_v 25 | if (row_h % row_v != 0) 26 | // Cannot create hankel matrix 27 | return -EINVAL; 28 | 29 | memset(H, 0, row_h * column_h * sizeof(float)); 30 | 31 | uint16_t delta = 0; 32 | 33 | for (uint16_t i = 0; i < row_h / row_v; i++) { 34 | for (uint16_t j = 0; j < row_v; j++) { 35 | // Compute how much we should load H for every row 36 | if (column_v > column_h + i) 37 | delta = column_v - column_h; 38 | else 39 | delta = column_h - i; 40 | 41 | // We cannot overindexing the V array. Lower the delta 42 | if ((row_v - 1) * column_v + i + shift + delta > row_v * column_v) { 43 | delta--; 44 | } 45 | 46 | memcpy(H + (i * row_v + j) * column_h, V + j * column_v + i + shift, 47 | delta * sizeof(float)); 48 | } 49 | } 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /src/linalg/linsolve_chol.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | void linsolve_chol(const float *const A, float *x, const float *const b, uint16_t row) 13 | { 14 | float L[row * row]; 15 | float y[row]; 16 | 17 | chol(A, L, row); 18 | linsolve_lower_triangular(L, y, b, row); 19 | tran(L, L, row, row); 20 | linsolve_upper_triangular(L, x, y, row); 21 | } 22 | -------------------------------------------------------------------------------- /src/linalg/linsolve_lower_triangular.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | 14 | /* 15 | * Solve with forward substitution. This can be used with Cholesky decomposition 16 | * A [m*n] need to be lower triangular and square 17 | * b [m] 18 | * x [n] 19 | * n == m 20 | */ 21 | void linsolve_lower_triangular(const float *const A, float x[], const float *const b, uint16_t row) 22 | { 23 | // Time to solve x from Ax = b. 24 | memset(x, 0, row * sizeof(float)); 25 | 26 | for (uint16_t i = 0; i < row; i++) { 27 | float sum = 0; 28 | 29 | for (uint16_t j = 0; j < i; j++) { 30 | sum += A[i * row + j] * x[j]; 31 | } 32 | 33 | x[i] = (b[i] - sum) / A[row * i + i]; 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /src/linalg/linsolve_lup.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | 14 | int linsolve_lup(const float *const A, float *x, const float *const b, uint16_t row) 15 | { 16 | float LU[row * row]; 17 | uint8_t P[row]; 18 | 19 | if (lup(A, LU, P, row) != 0) { 20 | return -ENOTSUP; 21 | } 22 | 23 | // forward substitution with pivoting 24 | for (int i = 0; i < row; ++i) { 25 | x[i] = b[P[i]]; 26 | 27 | for (int j = 0; j < i; ++j) { 28 | x[i] = x[i] - LU[row * P[i] + j] * x[j]; 29 | } 30 | } 31 | 32 | // backward substitution with pivoting 33 | for (int i = row - 1; i >= 0; --i) { 34 | for (int j = i + 1; j < row; ++j) { 35 | x[i] = x[i] - LU[row * P[i] + j] * x[j]; 36 | } 37 | 38 | x[i] = x[i] / LU[row * P[i] + i]; 39 | 40 | if (i == 0) { 41 | break; 42 | } 43 | } 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /src/linalg/linsolve_markov.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/sysid.h" 11 | 12 | #include 13 | 14 | void linsolve_markov(float *g, const float *const y, const float *const u, uint16_t row, 15 | uint16_t column) 16 | { 17 | memset(g, 0, row * column * sizeof(float)); 18 | 19 | // If we have more than 1 rows = MIMO system 20 | for (uint16_t k = 0; k < row; k++) { 21 | for (uint16_t i = 0; i < column; i++) { 22 | float sum = 0; 23 | for (int j = 0; j < i; j++) { 24 | sum += u[k * column + i - j] * g[k * column + j]; 25 | } 26 | g[k * column + i] = (y[k * column + i] - sum) / u[k * column + 0]; 27 | } 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /src/linalg/linsolve_qr.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include 11 | 12 | void linsolve_qr(const float *const A, float *x, const float *const b, uint16_t row, 13 | uint16_t column) 14 | { 15 | // QR-decomposition 16 | float Q[row * row]; 17 | float R[row * column]; 18 | float QTb[row]; 19 | 20 | qr(A, Q, R, row, column, false); 21 | tran(Q, Q, row, row); // Do transpose Q -> Q^T 22 | mul(QTb, Q, b, row, row, row, 1); // Q^Tb = Q^T*b 23 | linsolve_upper_triangular(R, x, QTb, column); 24 | } 25 | -------------------------------------------------------------------------------- /src/linalg/linsolve_upper_triangular.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | 14 | /* 15 | * This solves Ax = b. 16 | * A need to be square and upper triangular. 17 | * A [m*n] 18 | * b [m] 19 | * m == n 20 | */ 21 | void linsolve_upper_triangular(const float *const A, float x[], const float *const b, 22 | uint16_t column) 23 | { 24 | // Time to solve x from Ax = b. 25 | memset(x, 0, column * sizeof(float)); 26 | 27 | // Column 28 | for (int i = column - 1; i >= 0; i--) { 29 | float sum = 0.0f; // This is our sum 30 | // Row 31 | for (int j = i; j < column; j++) { 32 | sum += A[i * column + j] * x[j]; 33 | } 34 | x[i] = (b[i] - sum) / A[i * column + i]; 35 | 36 | if (i == 0) { 37 | break; 38 | } 39 | } 40 | } 41 | 42 | /* 43 | * GNU Octave code: 44 | * function [x] = linsolve(A, b) 45 | s = length(A); 46 | x = zeros(s,1); 47 | x(s) = b(s)/A(s,s); 48 | for i = s-1:-1:1 49 | sum = 0; 50 | for j = s:-1:i+1 51 | sum = sum + A(i,j)*x(j); 52 | end 53 | x(i) = (b(i)- sum)/A(i,i); 54 | end 55 | end 56 | */ 57 | -------------------------------------------------------------------------------- /src/linalg/lup.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | int lup(const float *const A, float *LU, uint8_t *P, uint16_t row) 18 | { 19 | // If not the same 20 | if (A != LU) 21 | memcpy(LU, A, row * row * sizeof(float)); 22 | 23 | // Create the pivot vector 24 | for (uint16_t i = 0; i < row; ++i) { 25 | P[i] = i; 26 | } 27 | 28 | for (uint16_t i = 0; i < row - 1; ++i) { 29 | uint16_t ind_max = i; 30 | 31 | for (uint16_t j = i + 1; j < row; ++j) 32 | if (fabsf(LU[row * P[j] + i]) > fabsf(LU[row * P[ind_max] + i])) 33 | ind_max = j; 34 | 35 | uint16_t tmp_int = P[i]; 36 | 37 | P[i] = P[ind_max]; 38 | P[ind_max] = tmp_int; 39 | 40 | if (fabsf(LU[row * P[i] + i]) < FLT_EPSILON) 41 | return -ENOTSUP; // matrix is singular (up to tolerance) 42 | 43 | for (uint16_t j = i + 1; j < row; ++j) { 44 | LU[row * P[j] + i] = LU[row * P[j] + i] / LU[row * P[i] + i]; 45 | 46 | for (uint16_t k = i + 1; k < row; ++k) 47 | LU[row * P[j] + k] = LU[row * P[j] + k] - 48 | LU[row * P[i] + k] * LU[row * P[j] + i]; 49 | } 50 | } 51 | 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /src/linalg/mul.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | 14 | int mul(float *C, const float *const A, const float *const B, uint16_t row_a, uint16_t column_a, 15 | uint16_t row_b, uint16_t column_b) 16 | { 17 | // Data matrix 18 | const float *data_a; 19 | const float *data_b; 20 | 21 | if (column_a != row_b) { 22 | return -EINVAL; 23 | } 24 | 25 | for (uint16_t i = 0; i < row_a; i++) { 26 | // Then we go through every column of b 27 | for (uint16_t j = 0; j < column_b; j++) { 28 | data_a = &A[i * column_a]; 29 | data_b = &B[j]; 30 | 31 | *C = 0; // Reset 32 | // And we multiply rows from a with columns of b 33 | for (uint16_t k = 0; k < column_a; k++) { 34 | *C += *data_a * *data_b; 35 | data_a++; 36 | data_b += column_b; 37 | } 38 | C++; 39 | } 40 | } 41 | return 0; 42 | } 43 | 44 | /* 45 | * GNU Octave code: 46 | * >> A = [4 23; 2 5]; 47 | >> B = [3; 1]; 48 | >> C = A*B 49 | C = 50 | 51 | 35 52 | 11 53 | 54 | >> 55 | * 56 | */ 57 | -------------------------------------------------------------------------------- /src/linalg/pinv.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | void pinv(float *Ai, const float *const A, uint16_t row, uint16_t column) 13 | { 14 | // Use Golub and Reinch if row != column 15 | float U[row * column]; 16 | float S[column]; 17 | float V[column * column]; 18 | 19 | if (row == column) 20 | svd_jacobi_one_sided(A, row, MAX_ITERATION_COUNT_SVD, U, S, V); 21 | else 22 | svd_golub_reinsch(A, row, column, U, S, V); 23 | 24 | // Do inv(S) 25 | for (uint16_t i = 0; i < column; i++) { 26 | // Create inverse diagonal matrix 27 | // TODO: can this ever result in div by zero? 28 | S[i] = 1.0f / S[i]; 29 | } 30 | 31 | // Transpose U' 32 | tran(U, U, row, column); 33 | 34 | // U = S*U' 35 | for (uint16_t i = 0; i < row; i++) { 36 | for (uint16_t j = 0; j < column; j++) { 37 | U[row * j + i] = S[j] * U[row * j + i]; 38 | } 39 | } 40 | 41 | // Do pinv now: A = V*U 42 | mul(Ai, V, U, column, column, column, row); 43 | } 44 | -------------------------------------------------------------------------------- /src/linalg/sum.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | 14 | void sum(float *Ar, const float *const A, uint16_t row, uint16_t column, uint8_t l) 15 | { 16 | memcpy(Ar, A, sizeof(float) * row * column); 17 | if (l == 1) { 18 | for (uint16_t i = 1; i < row; i++) { 19 | for (uint16_t j = 0; j < column; j++) { 20 | Ar[j] += A[i * column + j]; 21 | } 22 | } 23 | } else if (l == 2) { 24 | for (uint16_t i = 0; i < row; i++) { 25 | for (uint16_t j = 1; j < column; j++) { 26 | Ar[i * column] += A[i * column + j]; 27 | } 28 | } 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /src/linalg/svd_jacobi_one_sided.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/swedishembedded/control/5327a24485f246f20cc79f98427bd330cb8c2b37/src/linalg/svd_jacobi_one_sided.c -------------------------------------------------------------------------------- /src/linalg/tran.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/linalg.h" 11 | 12 | #include 13 | 14 | void tran(float *At, const float *const A, uint16_t row, uint16_t column) 15 | { 16 | float B[row * column]; 17 | const float *ptr_A = A; 18 | 19 | for (uint16_t i = 0; i < row; i++) { 20 | float *transpose = &B[i]; 21 | 22 | for (uint16_t j = 0; j < column; j++) { 23 | *transpose = *ptr_A; 24 | ptr_A++; 25 | transpose += row; 26 | } 27 | } 28 | 29 | // Copy! 30 | memcpy(At, B, row * column * sizeof(float)); 31 | } 32 | -------------------------------------------------------------------------------- /src/misc/cat.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/misc.h" 11 | 12 | #include 13 | #include 14 | 15 | void cat(float *C, const float *const A, const float *const B, int vertical, uint16_t row_a, 16 | uint16_t column_a, uint16_t row_b, uint16_t column_b, uint16_t row_c, uint16_t column_c) 17 | { 18 | if (vertical == 1) { 19 | /* C = [A;B] */ 20 | assert(column_a == column_b); 21 | assert(row_a + row_b == row_c); 22 | memcpy(C, A, row_a * column_a * sizeof(float)); 23 | memcpy(C + row_a * column_a, B, row_b * column_b * sizeof(float)); 24 | } else { 25 | /* C = [A, B] */ 26 | assert(row_a == row_b); 27 | assert(column_a + column_b == column_c); 28 | memcpy(C, A, column_a * sizeof(float)); 29 | memcpy(C + column_a, B, column_b * sizeof(float)); 30 | memcpy(C + column_a + column_b, A + column_a, column_a * sizeof(float)); 31 | memcpy(C + column_a + column_b + column_a, B + column_b, column_b * sizeof(float)); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/misc/constrain.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/misc.h" 11 | 12 | float constrain_float(float input, float lower_limit, float upper_limit) 13 | { 14 | if (input > upper_limit) { 15 | return upper_limit; 16 | } 17 | 18 | if (input < lower_limit) { 19 | return lower_limit; 20 | } 21 | // unchanged 22 | return input; 23 | } 24 | 25 | void constrain(float *I, const float *const Iin, uint16_t dim, float lower, float upper) 26 | { 27 | for (unsigned int i = 0; i < dim; i++) { 28 | I[i] = constrain_float(Iin[i], lower, upper); 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /src/misc/cut.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/misc.h" 11 | 12 | #include 13 | 14 | void cut(float *B, const float *const A, uint16_t row, uint16_t column, uint16_t start_row, 15 | uint16_t start_column, uint16_t row_b, uint16_t column_b) 16 | { 17 | (void)row; 18 | uint16_t stop_row = start_row + row_b; 19 | uint16_t stop_column = start_column + column_b; 20 | int in_columns = column; 21 | const float *data = A + start_row * in_columns + start_column; 22 | 23 | // Create the output 24 | int out_columns = stop_column - start_column; 25 | 26 | // Instead of having two for loops, we just copy the whole row at once. 27 | for (int i = start_row; i < stop_row; i++) { 28 | memcpy(B, data, sizeof(float) * out_columns); 29 | B += out_columns; 30 | data += in_columns; 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /src/misc/insert.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/misc.h" 11 | 12 | #include 13 | 14 | void insert(float *B, const float *const A, uint16_t row_a, uint16_t column_a, uint16_t column_b, 15 | uint16_t startRow_b, uint16_t startColumn_b) 16 | { 17 | const float *Apos = A; 18 | 19 | // Now we are standing on position startRow_b x startColumn_b of row_b x column_b 20 | B += startRow_b * column_b + startColumn_b; 21 | 22 | // We start at startRow_b and end at row_a, then we have inserted all rows from A into B 23 | for (uint16_t i = 0; i < row_a; i++) { 24 | memcpy(B, Apos, column_a * sizeof(float)); // Insert the complete row from A into B 25 | Apos += column_a; 26 | B += column_b; 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /src/misc/mean.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/misc.h" 11 | 12 | float mean(const float *const x, uint16_t length) 13 | { 14 | float s = 0; 15 | 16 | for (uint16_t i = 0; i < length; i++) 17 | s += x[i]; 18 | 19 | return s / ((float)length); 20 | } 21 | -------------------------------------------------------------------------------- /src/misc/print.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/misc.h" 11 | 12 | #include 13 | 14 | /* 15 | * Print matrix or vector - Just for error check 16 | */ 17 | void print(float A[], uint16_t row, uint16_t column) 18 | { 19 | for (uint16_t i = 0; i < row; i++) { 20 | for (uint16_t j = 0; j < column; j++) { 21 | printf("%c%0.18f\t", (j) ? ',' : ' ', *(A++)); 22 | } 23 | printf(";\n"); 24 | } 25 | printf("\n"); 26 | } 27 | -------------------------------------------------------------------------------- /src/misc/randn.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/misc.h" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | static float generate_gauss(float mu, float sigma) 18 | { 19 | float U1, U2, W, scalar; 20 | static float X1, X2; 21 | static int call; 22 | 23 | if (call == 1) { 24 | call = !call; 25 | return (mu + sigma * X2); 26 | } 27 | 28 | // Compute the uniform norm 29 | do { 30 | U1 = -1.0f + ((float)rand() / (float)RAND_MAX) * 2.0f; 31 | U2 = -1.0f + ((float)rand() / (float)RAND_MAX) * 2.0f; 32 | W = powf(U1, 2) + powf(U2, 2); 33 | } while (W >= 1.0f || W == 0.0f); 34 | 35 | // TODO: potential division by zero 36 | scalar = sqrtf((-2.0f * logf(W)) / W); 37 | X1 = U1 * scalar; 38 | X2 = U2 * scalar; 39 | 40 | call = !call; 41 | 42 | return (mu + sigma * X1); 43 | } 44 | 45 | void randn(float *x, uint16_t length, float mu, float sigma) 46 | { 47 | for (uint16_t i = 0; i < length; i++) 48 | x[i] = generate_gauss(mu, sigma); 49 | } 50 | -------------------------------------------------------------------------------- /src/misc/sign.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include 11 | 12 | float sign(float number) 13 | { 14 | float s; 15 | 16 | if (number > 0) 17 | s = 1.0f; 18 | else if (number < 0) 19 | s = -1.0f; 20 | else 21 | s = 0.0f; 22 | return s; 23 | } 24 | -------------------------------------------------------------------------------- /src/misc/stddev.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2019 Daniel Mårtensson 4 | * Copyright 2022 Martin Schröder 5 | * Consulting: https://swedishembedded.com/consulting 6 | * Simulation: https://swedishembedded.com/simulation 7 | * Training: https://swedishembedded.com/training 8 | */ 9 | 10 | #include "control/misc.h" 11 | 12 | #include 13 | 14 | /* 15 | * Compute Standard deviation 16 | * x[L] Vector with values 17 | * L = Length of vector x 18 | */ 19 | float stddev(const float *const x, uint16_t length) 20 | { 21 | float mu = mean(x, length); 22 | float sigma = 0; 23 | 24 | for (uint16_t i = 0; i < length; i++) { 25 | sigma += (x[i] - mu) * (x[i] - mu); 26 | } 27 | 28 | return sqrtf(sigma / ((float)length - 1)); 29 | } 30 | -------------------------------------------------------------------------------- /src/model/dc_motor.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Copyright 2022 Martin Schröder 3 | % Consulting: https://swedishembedded.com/go 4 | % Training: https://swedishembedded.com/tag/training 5 | 6 | dir = fileparts(mfilename('fullpath')); 7 | path(strcat(dir, "/../../octave/"), path); 8 | 9 | pkg load control 10 | pkg load symbolic 11 | 12 | # Step 1: start with a continuous time state space model 13 | syms s J b K R L z Ts 14 | A = [-b/J K/J; 15 | -K/L -R/L]; 16 | B = [0; 17 | 1/L]; 18 | C = [1 0]; 19 | D = 0; 20 | sys = ss(A, B, C, D); 21 | 22 | # Step 2: transform into a continuous time transfer function 23 | Hz = tf(sys, Ts) 24 | 25 | # Step 4: extract the difference equation 26 | [N, D] = numden(Hz) 27 | 28 | # Step 5: generate C code 29 | ccode(Hz) 30 | -------------------------------------------------------------------------------- /src/motor/clarke.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/consulting 5 | * Simulation: https://swedishembedded.com/simulation 6 | * Training: https://swedishembedded.com/training 7 | */ 8 | 9 | #include "control/motor.h" 10 | 11 | #include 12 | 13 | void clarke(float *xyz, const float *const abc) 14 | { 15 | xyz[0] = (2 * abc[0] - abc[1] - abc[2]) * (1.f / sqrtf(6.f)); 16 | xyz[1] = (abc[1] - abc[2]) * (1.f / sqrtf(2.f)); 17 | xyz[2] = (abc[0] + abc[1] + abc[2]) * (1.f / sqrtf(3.f)); 18 | } 19 | 20 | void inv_clarke(float *abc, const float *const xyz) 21 | { 22 | abc[0] = (1.f / sqrtf(3.f)) * xyz[2]; 23 | abc[1] = abc[0] - (1.f / sqrtf(6.f)) * xyz[0]; 24 | abc[2] = abc[1] - (1.f / sqrtf(2.f)) * xyz[1]; 25 | abc[1] += (1.f / sqrtf(2.f)) * xyz[1]; 26 | abc[0] += (sqrtf(2.f / 3.f)) * xyz[0]; 27 | } 28 | -------------------------------------------------------------------------------- /src/motor/park.c: -------------------------------------------------------------------------------- 1 | // SPDX-License-Identifier: MIT 2 | /** 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/consulting 5 | * Simulation: https://swedishembedded.com/simulation 6 | * Training: https://swedishembedded.com/training 7 | */ 8 | 9 | #include "control/motor.h" 10 | 11 | #include 12 | 13 | void park(float *dqz, const float *const xyz, const float angle) 14 | { 15 | float co = cosf(angle); 16 | float si = sinf(angle); 17 | 18 | dqz[0] = co * xyz[0] + si * xyz[1]; 19 | dqz[1] = co * xyz[1] - si * xyz[0]; 20 | dqz[2] = xyz[2]; 21 | } 22 | 23 | void inv_park(float *xyz, const float *const dqz, const float angle) 24 | { 25 | float co = cosf(angle); 26 | float si = sinf(angle); 27 | 28 | xyz[0] = co * dqz[0] - si * dqz[1]; 29 | xyz[1] = si * dqz[0] + co * dqz[1]; 30 | xyz[2] = dqz[2]; 31 | } 32 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(define_test FUNC) 2 | add_executable(${FUNC} main.cpp) 3 | target_include_directories(${FUNC} PRIVATE ${CMAKE_BINARY_DIR}/tests/sym_tf) 4 | target_link_libraries(${FUNC} control gtest pthread) 5 | add_test(NAME ${FUNC} COMMAND ${FUNC}) 6 | endfunction() 7 | 8 | add_subdirectory(ai) 9 | add_subdirectory(filter) 10 | add_subdirectory(motion) 11 | add_subdirectory(misc) 12 | add_subdirectory(model) 13 | add_subdirectory(linalg) 14 | add_subdirectory(dynamics) 15 | add_subdirectory(optimization) 16 | add_subdirectory(sysid) 17 | add_subdirectory(motor) 18 | -------------------------------------------------------------------------------- /tests/ai/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | 4 | define_test(ai) 5 | target_sources(ai PRIVATE main.cpp) 6 | target_sources(ai PRIVATE a_star.cpp) 7 | target_sources(ai PRIVATE inpolygon.cpp) 8 | -------------------------------------------------------------------------------- /tests/ai/inpolygon.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/dynamics.h" 14 | #include "control/ai.h" 15 | }; 16 | 17 | TEST(Main, inpolygon) 18 | { 19 | /* Create the polygon coordinates */ 20 | float px[12] = { 47.3364, 59.7788, 65.0323, 62.5438, 51.4839, 38.7650, 21 | 36.5530, 36.5530, 37.1060, 40.1475, 46.5069, 47.3364 }; 22 | float py[12] = { -3.2484, 6.8953, 22.4731, 32.2546, 39.5001, 33.3414, 23 | 18.8504, 5.4462, -3.9730, -11.9430, -12.3053, -3.2484 }; 24 | uint8_t p = 12; 25 | 26 | /* Check if this coordinate is inside the polygon */ 27 | float x = 62.035; 28 | float y = 22.473; 29 | 30 | /* Perform algorithm */ 31 | uint8_t is_inside = inpolygon(x, y, px, py, p); 32 | ASSERT_TRUE(is_inside); 33 | } 34 | 35 | /* Gnu Octave code: 36 | * 37 | * >> XV = [47.3364, 59.7788, 65.0323, 62.5438, 51.4839, 38.7650, 36.5530, 36.5530, 37.1060, 40.1475, 46.5069, 47.3364]; 38 | >> YV = [-3.2484, 6.8953, 22.4731, 32.2546, 39.5001, 33.3414, 18.8504, 5.4462, -3.9730, -11.9430, -12.3053, -3.2484]; 39 | >> inpolygon (65.034, 22.473, XV, YV) 40 | ans = 0 41 | >> inpolygon (65.032, 22.473, XV, YV) 42 | ans = 1 43 | >> 44 | */ 45 | -------------------------------------------------------------------------------- /tests/ai/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/dynamics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | 4 | define_test(dynamics) 5 | target_sources(dynamics PRIVATE main.cpp) 6 | target_sources(dynamics PRIVATE c2d.cpp) 7 | target_sources(dynamics PRIVATE kalman.cpp) 8 | target_sources(dynamics PRIVATE lqi.cpp) 9 | target_sources(dynamics PRIVATE mpc.cpp) 10 | target_sources(dynamics PRIVATE mrac.cpp) 11 | target_sources(dynamics PRIVATE pid.cpp) 12 | target_sources(dynamics PRIVATE stability.cpp) 13 | target_sources(dynamics PRIVATE theta2ss.cpp) 14 | -------------------------------------------------------------------------------- /tests/dynamics/c2d.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | extern "C" { 12 | #include "control/dynamics.h" 13 | }; 14 | 15 | TEST(Main, C2D) 16 | { 17 | // clang-format off 18 | float A_exp[] = {51.968956, 74.736565, 112.10485, 164.07379}; 19 | float B_exp[] = {13.600674, 30.567947}; 20 | float A[] = { 1, 2, 3, 4 }; 21 | float B[] = {0, 1}; 22 | float Ad[4]; 23 | float Bd[2]; 24 | 25 | // clang-format on 26 | 27 | c2d(Ad, Bd, A, B, 2, 1, 1); 28 | 29 | for (unsigned int c = 0; c < 4; c++) { 30 | ASSERT_FLOAT_EQ(A_exp[c], Ad[c]); 31 | } 32 | 33 | for (unsigned int c = 0; c < 2; c++) { 34 | ASSERT_FLOAT_EQ(B_exp[c], Bd[c]); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /tests/dynamics/kalman.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/dynamics.h" 14 | }; 15 | 16 | TEST(Main, Kalman) 17 | { 18 | #define ADIM 2 19 | #define RDIM 2 20 | #define YDIM 2 21 | 22 | // clang-format off 23 | float A[ADIM * ADIM] = { 0.79499, 0.33654, -0.70673, 0.35749 }; 24 | float B[ADIM * RDIM] = { 0.2050072, 0.0097622, 0.7067279, 0.0336537 }; 25 | float C[YDIM * ADIM] = { 1.20000, 0.00000, 0.00000, 0.30000 }; 26 | float K[ADIM * YDIM] = { 0.532591, -0.066108, -0.503803, 0.136580 }; 27 | // clang-format on 28 | 29 | // Create input u, output y and state vector x 30 | float u[RDIM] = { 2.2, 4.3 }; 31 | float y[YDIM] = { 5.134, 0.131 }; 32 | float x[ADIM] = { 2.32, 4.12 }; 33 | 34 | // Call kalman function and estimate our future state at x(k+1) 35 | kalman(x, A, x, B, u, K, y, C, ADIM, YDIM, RDIM); 36 | #undef RDIM 37 | #undef YDIM 38 | #undef ADIM 39 | 40 | float x_exp[] = { 5.04855, 0.19790 }; 41 | 42 | for (unsigned int c = 0; c < 2; c++) { 43 | ASSERT_NEAR(x_exp[c], x[c], 1e-3); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /tests/dynamics/kalman.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | %% Example made by Daniel Mårtensson - 2019-10-08 3 | %% To run this example. You need to install Matavecontrol 4 | 5 | % Real life model 6 | sys = ss(0, [0 1; -2.1 -1.3], [0 0; 2.1 0.1], [1.2 0; 0 0.3]); 7 | 8 | % Turn it to discrete with sample time h = 0.5 9 | sysd = c2d(sys, 0.5); 10 | 11 | % Kompute the kalman gain matrix with Q = 2*eye(2), R = 1.3*[1 2; 1 5] 12 | Q = 2*eye(2); 13 | R = 1.3*[1 2; 1 5]; 14 | K = lqe(sysd, Q, R); 15 | 16 | % Create input u, output y and state vector x 17 | u = [2.2; 18 | 4.3]; 19 | 20 | y = [5.134; 21 | 0.131]; 22 | 23 | x = [2.32; 24 | 4.12]; 25 | 26 | disp('Our current state vector is:') 27 | x 28 | 29 | % Estimate our future state at x(k+1) 30 | x = sysd.A*x - K*sysd.C*x + sysd.B*u + K*y; 31 | disp('Our estimated state vector is:') 32 | x 33 | -------------------------------------------------------------------------------- /tests/dynamics/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/dynamics/mpc.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/dynamics.h" 14 | #include "control/linalg.h" 15 | }; 16 | 17 | TEST(Main, MPC) 18 | { 19 | #define ADIM 2 20 | #define RDIM 1 21 | #define YDIM 1 22 | #define HORIZON 20 23 | #define ITERATION_LIMIT 200 24 | 25 | // State space model that have integration behavior 26 | float A[ADIM * ADIM] = { 1.71653, 1.00000, -0.71653, 0.00000 }; 27 | float B[ADIM * RDIM] = { 0.18699, 0.16734 }; 28 | float C[YDIM * ADIM] = { 1, 0 }; 29 | float x[ADIM] = { 0, 0 }; 30 | float u[RDIM] = { 0 }; 31 | float r[YDIM] = { 12.5 }; 32 | float K[ADIM] = { 0, 0 }; 33 | float y[YDIM] = { 0 }; 34 | 35 | // Do Model Predictive Control where we selecting last u 36 | for (int i = 0; i < 200; i++) { 37 | mpc(A, B, C, x, u, r, ADIM, YDIM, RDIM, HORIZON, ITERATION_LIMIT, 1); 38 | kalman(x, A, x, B, u, K, y, C, ADIM, YDIM, RDIM); 39 | mul(y, C, x, YDIM, ADIM, ADIM, 1); 40 | } 41 | 42 | EXPECT_NEAR(r[0], y[0], 1e-3); 43 | 44 | #undef ADIM 45 | #undef RDIM 46 | #undef YDIM 47 | #undef HORIZON 48 | #undef ITERATION_LIMIT 49 | } 50 | -------------------------------------------------------------------------------- /tests/dynamics/pid.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | extern "C" { 14 | #include "control/dynamics.h" 15 | #include "control/linalg.h" 16 | }; 17 | 18 | TEST(Main, PID) 19 | { 20 | // create a simple system with time constant 0.4 and gain 4 21 | // G = 4/(0.4*s + 1) 22 | // c2d(ss(G), 1) 23 | float A[] = { 0.778800783 }; 24 | float B[] = { 0.884796868 }; 25 | float C[] = { 1 }; 26 | float K[] = { 0 }; 27 | float r[] = { 4 }; 28 | float y[] = { 0 }; 29 | float u[] = { 0 }; 30 | float x[] = { 0 }; 31 | float Ts = 0.1; 32 | 33 | struct pid pid; 34 | pid_init(&pid); 35 | //pid_set_from_imc(&pid, 0.2, 4, 0.4, 0); 36 | pid_set_gains(&pid, 12.5, 3.125, 0, 0); 37 | 38 | for (unsigned int c = 0; c < 4; c++) { 39 | u[0] = pid_step(&pid, (r[0] - y[0]) * Ts); 40 | kalman(x, A, x, B, u, K, y, C, 1, 1, 1); 41 | mul(y, C, x, 1, 1, 1, 1); 42 | } 43 | 44 | // 0.6326 -> 63% of full scale 45 | float tau = 1.0f - expf(-1.f); 46 | 47 | // check that the system is following precisly its time constant 48 | EXPECT_NEAR(r[0] * tau, y[0] * tau, 0.05); 49 | } 50 | -------------------------------------------------------------------------------- /tests/dynamics/pid.m: -------------------------------------------------------------------------------- 1 | % SPDX-License-Identifier: MIT 2 | % Copyright 2022 Martin Schröder 3 | % Consulting: https://swedishembedded.com/go 4 | % Training: https://swedishembedded.com/tag/training 5 | 6 | pkg load control 7 | pkg load symbolic 8 | 9 | output_precision(9); 10 | 11 | Ts = 0.1 12 | 13 | function [p, i, d] = pid_imc(aggr, process_gain, time_constant, dead_time) 14 | tc = max((0.1 * aggr) * time_constant, (0.8 * aggr) * dead_time); 15 | kc = (1.0/process_gain)*((time_constant + 0.5 * dead_time)/(tc + 0.5 * dead_time)); 16 | ti = time_constant + 0.5 * dead_time; 17 | td = (time_constant * dead_time)/(2.0 * time_constant + dead_time); 18 | 19 | p = kc; 20 | i = kc / ti; 21 | d = kc * td; 22 | end 23 | 24 | s = tf('s'); 25 | z = tf('z', Ts); 26 | 27 | % System dynamics 28 | G = 4/(0.4*s + 1); 29 | Gd = ss(c2d(G, Ts)) 30 | Gd.a 31 | Gd.b 32 | Gd.c 33 | Gd.d 34 | % calculate ontinuous time gains 35 | [Kp, Ki, Kd] = pid_imc(0.2, 4, 0.4, 0) 36 | C = Kp + Kd * s + Ki / s 37 | 38 | % convert into discrete time 39 | Cd = c2d(C, Ts) 40 | sys_Cd = ss(Cd) 41 | sys_Cd.b 42 | sys_Cd.d 43 | -------------------------------------------------------------------------------- /tests/dynamics/stability.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/dynamics.h" 14 | #include "control/linalg.h" 15 | }; 16 | 17 | TEST(Main, IsStable) 18 | { 19 | // matrix with second eigenvalue > 1 20 | // eig(A) = -0.37228, 5.37228 21 | float A[2 * 2] = { 1, 2, 3, 4 }; 22 | EXPECT_FALSE(is_stable(A, 2)); 23 | float A2[2 * 2] = { 1, 0, -1, -1 }; 24 | EXPECT_TRUE(is_stable(A2, 2)); 25 | } 26 | -------------------------------------------------------------------------------- /tests/dynamics/theta2ss.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | extern "C" { 12 | #include "control/dynamics.h" 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, Theta2SS) 17 | { 18 | // TODO: write a test for this function 19 | } 20 | -------------------------------------------------------------------------------- /tests/filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | define_test(filter) 7 | target_sources(filter PRIVATE main.cpp) 8 | target_sources(filter PRIVATE filtfilt.cpp) 9 | target_sources(filter PRIVATE mcs.cpp) 10 | target_sources(filter PRIVATE sqr_ukf.cpp) 11 | -------------------------------------------------------------------------------- /tests/filter/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/linalg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | define_test(linalg) 7 | target_sources(linalg PRIVATE main.cpp) 8 | target_sources(linalg PRIVATE add.cpp) 9 | target_sources(linalg PRIVATE balance.cpp) 10 | target_sources(linalg PRIVATE chol.cpp) 11 | target_sources(linalg PRIVATE cholupdate.cpp) 12 | target_sources(linalg PRIVATE det.cpp) 13 | target_sources(linalg PRIVATE dlyap.cpp) 14 | target_sources(linalg PRIVATE eig.cpp) 15 | target_sources(linalg PRIVATE eig_sym.cpp) 16 | target_sources(linalg PRIVATE expm.cpp) 17 | target_sources(linalg PRIVATE hankel.cpp) 18 | target_sources(linalg PRIVATE inv.cpp) 19 | target_sources(linalg PRIVATE linsolve_chol.cpp) 20 | target_sources(linalg PRIVATE linsolve_gauss.cpp) 21 | target_sources(linalg PRIVATE linsolve_markov.cpp) 22 | target_sources(linalg PRIVATE linsolve_lower_triangular.cpp) 23 | target_sources(linalg PRIVATE linsolve_upper_triangular.cpp) 24 | target_sources(linalg PRIVATE linsolve_lup.cpp) 25 | target_sources(linalg PRIVATE linsolve_qr.cpp) 26 | target_sources(linalg PRIVATE lup.cpp) 27 | target_sources(linalg PRIVATE mul.cpp) 28 | target_sources(linalg PRIVATE nonlinsolve.cpp) 29 | target_sources(linalg PRIVATE norm.cpp) 30 | target_sources(linalg PRIVATE qr.cpp) 31 | target_sources(linalg PRIVATE pinv.cpp) 32 | target_sources(linalg PRIVATE sum.cpp) 33 | target_sources(linalg PRIVATE svd_golub_reinsch.cpp) 34 | target_sources(linalg PRIVATE svd_jacobi_one_sided.cpp) 35 | target_sources(linalg PRIVATE tran.cpp) 36 | -------------------------------------------------------------------------------- /tests/linalg/add.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, MatrixAddition) 17 | { 18 | float A[] = { 1, -2, 4, 5 }; 19 | float B[] = { -2, -8, 2, 1 }; 20 | float A_exp[] = { -1, -10, 6, 6 }; 21 | 22 | add(A, A, B, 2, 2); 23 | 24 | for (unsigned int c = 0; c < 4; c++) { 25 | ASSERT_FLOAT_EQ(A[c], A_exp[c]); 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /tests/linalg/balance.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, BalanceSquareMatrix) 17 | { 18 | float A[] = { -0.1, 10.0, -1.0, -5.0 }; 19 | float Abal[] = { -0.1, 2.5, -4.0, -5.0 }; 20 | balance(A, 2); 21 | for (unsigned int c = 0; c < 4; c++) { 22 | ASSERT_FLOAT_EQ(Abal[c], A[c]); 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /tests/linalg/chol.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, CholeskyDecomposition) 17 | { 18 | float A[] = { 5, 11, 11, 25 }; 19 | float L[4] = { 0 }; 20 | float Lt[4] = { 0 }; 21 | float Ar[4] = { 0 }; 22 | 23 | // perform cholesky decomposition 24 | chol(A, L, 2); 25 | // copy the L matrix and transpose the copy 26 | memcpy(Lt, L, sizeof(Lt)); 27 | tran(Lt, Lt, 2, 2); 28 | // multiply L with L transposed to get A 29 | mul(Ar, L, Lt, 2, 2, 2, 2); 30 | // check result 31 | for (unsigned int c = 0; c < 4; c++) { 32 | ASSERT_FLOAT_EQ(A[c], Ar[c]); 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /tests/linalg/cholupdate.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, CholeskyUpdate) 17 | { 18 | // A = pascal(3) 19 | float A[] = { 1, 1, 1, 1, 2, 3, 1, 3, 6 }; 20 | float L[9] = { 0 }; 21 | float Lxxt[9] = { 0 }; 22 | float x[3] = { 0, 0, 1 }; 23 | float xt[3] = { 0, 0, 1 }; 24 | float xxt[9] = { 0 }; 25 | float Axxt[9] = { 0 }; 26 | 27 | // perform cholesky decomposition 28 | // L = chol(A) 29 | chol(A, L, 3); 30 | // perform update using second cholesky 31 | // Lxxt = chol(A + x * x') 32 | tran(xt, xt, 3, 1); 33 | mul(xxt, x, xt, 3, 1, 1, 3); 34 | add(Axxt, A, xxt, 3, 3); 35 | chol(Axxt, Lxxt, 3); 36 | 37 | // perform the same operation using cholupdate 38 | // Lxxt = cholupdate(L, x) 39 | cholupdate(L, x, 3, 1); 40 | 41 | // Lxxt and Axxt should be the same 42 | for (unsigned int c = 0; c < 4; c++) { 43 | ASSERT_FLOAT_EQ(L[c], Lxxt[c]); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /tests/linalg/det.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, Determinant) 17 | { 18 | float A0[] = { 1, 2, 3, 4 }; 19 | float A1[] = { 23, -1, -40, 50 }; 20 | float A2[] = { 23 }; 21 | 22 | ASSERT_FLOAT_EQ(-2, det(A0, 2)); 23 | ASSERT_FLOAT_EQ(1110, det(A1, 2)); 24 | ASSERT_FLOAT_EQ(23, det(A2, 1)); 25 | } 26 | -------------------------------------------------------------------------------- /tests/linalg/dlyap.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, DiscreteLyapunov1) 17 | { 18 | float A[2 * 2] = { 0.798231, 0.191700, -0.575101, 0.031430 }; 19 | float Q[2 * 2] = { 1, 2, 2, 3 }; 20 | float P[2 * 2]; 21 | float Pex[2 * 2] = { 3.50103, 0.38501, 0.38501, 4.14811 }; 22 | 23 | dlyap(A, P, Q, 2); 24 | 25 | for (unsigned c = 0; c < 4; c++) { 26 | ASSERT_NEAR(Pex[c], P[c], 1e-5); 27 | } 28 | } 29 | 30 | TEST(Main, DiscreteLyapunov2) 31 | { 32 | float A[2 * 2] = { 11, 21, -5, -1 }; 33 | float Q[2 * 2] = { 1, 0, 0, 2 }; 34 | float P[2 * 2]; 35 | float Pex[2 * 2] = { -0.101202, 0.053004, 0.053004, -0.030258 }; 36 | 37 | dlyap(A, P, Q, 2); 38 | 39 | for (unsigned c = 0; c < 4; c++) { 40 | ASSERT_NEAR(Pex[c], P[c], 1e-5); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /tests/linalg/expm.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, MatrixExponential) 17 | { 18 | float A[2 * 2] = { 0.798231, 0.191700, -0.575101, 0.031430 }; 19 | float E_exp[2 * 2] = { 2.12609, 0.29204, -0.87611, 0.95795 }; 20 | 21 | expm(A, A, 2); 22 | 23 | for (unsigned c = 0; c < 4; c++) { 24 | ASSERT_NEAR(E_exp[c], A[c], 1e-5); 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /tests/linalg/linsolve_chol.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, LinsolveChol) 17 | { 18 | // Matrix A 19 | // clang-format off 20 | float A[5 * 5] = { 21 | 1.1000, 2.0000, 3.0000, 4.0000, 5.0000, 22 | 2.0000, 4.1000, 6.0000, 8.0000, 10.0000, 23 | 3.0000, 6.0000, 9.1000, 12.0000, 15.0000, 24 | 4.0000, 8.0000, 12.0000, 16.1000, 20.0000, 25 | 5.0000, 10.0000, 15.0000, 20.0000, 25.1000 }; 26 | // clang-format on 27 | 28 | // Vector b 29 | float b[5] = { 5, 3, 2, 5, 7 }; 30 | 31 | // Solution that need to have the same rows and columns from A 32 | float x[5]; 33 | 34 | float x_exp[5] = { 36.9328, 3.8657, -19.2015, -2.2686, 4.6642 }; 35 | 36 | // Do Cholesky decomposition and then solve with linsolve_chol 37 | linsolve_chol(A, x, b, 5); 38 | 39 | for (unsigned int c = 0; c < 5; c++) { 40 | EXPECT_NEAR(x_exp[c], x[c], 1e-3); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /tests/linalg/linsolve_lower_triangular.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, LinsolveLowerTriangular) 17 | { 18 | // currently already tested by LinsolveChol 19 | // TODO: add a specific test later 20 | } 21 | -------------------------------------------------------------------------------- /tests/linalg/linsolve_lup.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, LinsolveLup) 17 | { 18 | // Matrix A 19 | // clang-format off 20 | float A[4 * 4] = { 21 | 0.47462, 0.74679, 0.31008, 0.63073, 22 | 0.32540, 0.49584, 0.50932, 0.21492, 23 | 0.43855, 0.98844, 0.54041, 0.24647, 24 | 0.62808, 0.72591, 0.20244, 0.96743 }; 25 | 26 | // Vector b 27 | float b[4] = { 1.588964, 0.901248, 0.062029, 0.142180 }; 28 | // clang-format on 29 | 30 | // Solution that need to have the same rows and columns from A 31 | float x[4]; 32 | 33 | float x_exp[4] = { -44.1551, 6.1363, 15.1259, 21.0440 }; 34 | 35 | // Do LUP-decomposition and then solve with linsolve_lup 36 | ASSERT_EQ(0, linsolve_lup(A, x, b, 4)); 37 | 38 | for (unsigned int c = 0; c < 4; c++) { 39 | EXPECT_NEAR(x_exp[c], x[c], 1e-3); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /tests/linalg/linsolve_upper_triangular.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, LinsolveUpperTriangular) 17 | { 18 | // currently already tested by LinsolveChol 19 | // TODO: add a specific test later 20 | } 21 | -------------------------------------------------------------------------------- /tests/linalg/lup.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, Lup) 17 | { 18 | // Tested as part of linsolve_lup 19 | // TODO: add specific test cases for this 20 | } 21 | -------------------------------------------------------------------------------- /tests/linalg/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/linalg/mul.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, Mul) 17 | { 18 | float A[2 * 2] = { 1, 4, 2, 3 }; 19 | float x[2] = { 2, 1 }; 20 | float C[2]; 21 | float C_exp[2] = { 6, 7 }; 22 | 23 | mul(C, A, x, 2, 2, 2, 1); 24 | 25 | for (unsigned int c = 0; c < 2; c++) { 26 | EXPECT_NEAR(C_exp[c], C[c], 1e-3); 27 | } 28 | } 29 | 30 | TEST(Main, MulDifferentSize) 31 | { 32 | float A[2 * 3] = { 1, 2, 3, 4, 5, 6 }; 33 | float x[3 * 2] = { 2, 1, 3, 5, 3, 7 }; 34 | float C[2 * 2]; 35 | float C_exp[2 * 2] = { 17, 32, 41, 71 }; 36 | 37 | mul(C, A, x, 2, 3, 3, 2); 38 | 39 | for (unsigned int c = 0; c < 2 * 2; c++) { 40 | EXPECT_NEAR(C_exp[c], C[c], 1e-3); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /tests/linalg/norm.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, Norm) 17 | { 18 | float A[2 * 2] = { 1, 2, 3, 4 }; 19 | 20 | EXPECT_NEAR(6, norm(A, 2, 2, 1), 1e-4); 21 | EXPECT_NEAR(5.4650, norm(A, 2, 2, 2), 1e-4); 22 | } 23 | -------------------------------------------------------------------------------- /tests/linalg/sum.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | TEST(Main, Sum) 17 | { 18 | // clang-format off 19 | float A[3 * 3] = { 20 | 1, 2, 3, 21 | 4, 5, 6, 22 | 7, 8, 9 23 | }; 24 | float Ar1[3 * 3]; 25 | float Ar2[3 * 3]; 26 | float Ar1_exp[3 * 3] = { 27 | 12, 15, 18, 28 | 4, 5, 6, 29 | 7, 8, 9, 30 | }; 31 | float Ar2_exp[3 * 3] = { 32 | 6, 2, 3, 33 | 15, 5, 6, 34 | 24, 8, 9, 35 | }; 36 | // clang-format on 37 | 38 | sum(Ar1, A, 3, 3, 1); 39 | sum(Ar2, A, 3, 3, 2); 40 | 41 | for (unsigned int c = 0; c < 3 * 3; c++) { 42 | ASSERT_FLOAT_EQ(Ar1_exp[c], Ar1[c]); 43 | } 44 | for (unsigned int c = 0; c < 3 * 3; c++) { 45 | ASSERT_FLOAT_EQ(Ar2_exp[c], Ar2[c]); 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /tests/linalg/tran.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/linalg.h" 14 | }; 15 | 16 | /* 17 | * GNU Octave code: 18 | A = [4 23 5; 2 45 5] 19 | A = 20 | 21 | 4 23 5 22 | 2 45 5 23 | 24 | >> A' 25 | ans = 26 | 27 | 4 2 28 | 23 45 29 | 5 5 30 | */ 31 | 32 | TEST(Main, Tran) 33 | { 34 | // cmake-format off 35 | float A[2 * 2] = { 1, 2, 3, 4 }; 36 | float At[2 * 2]; 37 | float At_exp[2 * 2] = { 1, 3, 2, 4 }; 38 | // cmake-format on 39 | 40 | tran(At, A, 2, 2); 41 | 42 | for (unsigned int c = 0; c < 2 * 2; c++) { 43 | ASSERT_FLOAT_EQ(At_exp[c], At[c]); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /tests/misc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | define_test(misc) 7 | target_sources(misc PRIVATE main.cpp) 8 | target_sources(misc PRIVATE cat.cpp) 9 | target_sources(misc PRIVATE cut.cpp) 10 | target_sources(misc PRIVATE insert.cpp) 11 | target_sources(misc PRIVATE mean.cpp) 12 | target_sources(misc PRIVATE print.cpp) 13 | target_sources(misc PRIVATE randn.cpp) 14 | target_sources(misc PRIVATE stddev.cpp) 15 | target_sources(misc PRIVATE constrain.cpp) 16 | target_sources(misc PRIVATE sign.cpp) 17 | -------------------------------------------------------------------------------- /tests/misc/cat.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/misc.h" 14 | }; 15 | 16 | TEST(Main, Cat) 17 | { 18 | // clang-format off 19 | float A[2 * 3] = { 20 | 1, 2, 3, 21 | 4, 5, 6 22 | }; 23 | float B[2 * 4] = { 24 | 7, 8, 9, 6, 25 | 1, 2, 3, 5 26 | }; 27 | float C[2 * 7] = { 0 }; 28 | float C_exp[2 * 7] = { 29 | 1, 2, 3, 7, 8, 9, 6, 30 | 4, 5, 6, 1, 2, 3, 5 31 | }; 32 | // clang-format on 33 | 34 | cat(C, A, B, 0, 2, 3, 2, 4, 2, 7); /* do C = [A B] */ 35 | 36 | for (unsigned int c = 0; c < 2 * 7; c++) { 37 | ASSERT_FLOAT_EQ(C_exp[c], C[c]); 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /tests/misc/constrain.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/misc.h" 14 | }; 15 | 16 | TEST(Main, Constrain) 17 | { 18 | float x[3] = { 4, 2, -1 }; 19 | float x_out[3] = { 0 }; 20 | EXPECT_FLOAT_EQ(4, constrain_float(8, 0, 4)); 21 | EXPECT_FLOAT_EQ(0, constrain_float(-1, 0, 4)); 22 | EXPECT_FLOAT_EQ(1, constrain_float(1, 0, 4)); 23 | constrain(x_out, x, 3, 1, 2); 24 | EXPECT_FLOAT_EQ(x_out[0], 2); 25 | EXPECT_FLOAT_EQ(x_out[2], 1); 26 | } 27 | -------------------------------------------------------------------------------- /tests/misc/cut.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/dynamics.h" 14 | #include "control/linalg.h" 15 | #include "control/misc.h" 16 | }; 17 | 18 | TEST(Main, Cut) 19 | { 20 | // clang-format off 21 | float A[5 * 5] = { 22 | 0.87508, 0.48607, 0.30560, 0.32509, 0.23096, 23 | 0.12308, 0.84311, 0.42221, 0.20273, 0.87377, 24 | 0.42986, 0.11245, 0.40494, 0.27304, 0.59772, 25 | 0.85124, 0.99245, 0.56873, 0.19438, 0.11308, 26 | 0.97190, 0.22475, 0.24501, 0.85403, 0.54691 27 | }; 28 | // clang-format on 29 | 30 | // Matrix B 31 | float B[2 * 3]; 32 | 33 | // Cut a matrix 34 | // We want to B = A(4:5, 3:5) 35 | cut(B, A, 5, 5, 3, 2, 2, 3); 36 | 37 | printf("B\n"); 38 | print(B, 2, 3); 39 | 40 | for (unsigned int i = 3; i < 5; i++) { 41 | for (unsigned int j = 2; j < 5; j++) { 42 | EXPECT_FLOAT_EQ(A[i * 5 + j], B[(i - 3) * 3 + (j - 2)]); 43 | } 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /tests/misc/insert.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/misc.h" 14 | }; 15 | 16 | TEST(Main, Insert) 17 | { 18 | // clang-format off 19 | float B[5 * 5] = { 20 | 0.87508, 0.48607, 0.30560, 0.32509, 0.23096, 21 | 0.12308, 0.84311, 0.42221, 0.20273, 0.87377, 22 | 0.42986, 0.11245, 0.40494, 0.27304, 0.59772, 23 | 0.85124, 0.99245, 0.56873, 0.19438, 0.11308, 24 | 0.97190, 0.22475, 0.24501, 0.85403, 0.54691 25 | }; 26 | float A[2 * 3] = { 27 | 1, 2, 3, 28 | 4, 5, 6 29 | }; 30 | // clang-format on 31 | 32 | // Paste A in position 2 2 in B 33 | insert(B, A, 2, 3, 5, 2, 2); 34 | 35 | printf("B\n"); 36 | print(B, 5, 5); 37 | 38 | for (unsigned int i = 0; i < 2; i++) { 39 | for (unsigned int j = 0; j < 3; j++) { 40 | EXPECT_FLOAT_EQ(A[i * 3 + j], B[(2 + i) * 5 + (2 + j)]); 41 | } 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /tests/misc/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/misc/mean.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/misc.h" 14 | }; 15 | 16 | TEST(Main, Mean) 17 | { 18 | float A[4] = { 1, -4, 2, 4 }; 19 | EXPECT_NEAR(0.75, mean(A, 4), 1e-4); 20 | } 21 | -------------------------------------------------------------------------------- /tests/misc/print.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/misc.h" 14 | }; 15 | 16 | TEST(Main, Print) 17 | { 18 | // NOTE: this test is mostly just a dummy 19 | // Print is mostly used for testing and debugging anyway 20 | float A[4] = { 1, -4, 2, 4 }; 21 | print(A, 2, 2); 22 | } 23 | -------------------------------------------------------------------------------- /tests/misc/randn.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/misc.h" 14 | }; 15 | 16 | TEST(Main, Randn) 17 | { 18 | float x[10]; 19 | randn(x, 10, 4, 2); 20 | EXPECT_NEAR(4, mean(x, 10), 1); 21 | EXPECT_NEAR(2, stddev(x, 10), 1); 22 | } 23 | -------------------------------------------------------------------------------- /tests/misc/sign.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/misc.h" 14 | }; 15 | 16 | TEST(Main, Sign) 17 | { 18 | EXPECT_FLOAT_EQ(-1, sign(-10)); 19 | EXPECT_FLOAT_EQ(1, sign(123)); 20 | EXPECT_FLOAT_EQ(0, sign(0)); 21 | } 22 | -------------------------------------------------------------------------------- /tests/misc/stddev.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Copyright 2017 Daniel Mårtensson 5 | * Consulting: https://swedishembedded.com/go 6 | * Training: https://swedishembedded.com/tag/training 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "control/misc.h" 14 | }; 15 | 16 | TEST(Main, Stddev) 17 | { 18 | // clang-format off 19 | float x[10] = { 20 | 3, 11, -8, 1, 4, 21 | 8, 9, 2, 5, 3 22 | }; 23 | // clang-format on 24 | EXPECT_NEAR(3.8000, mean(x, 10), 1e-2); 25 | EXPECT_NEAR(5.2662, stddev(x, 10), 1e-2); 26 | } 27 | -------------------------------------------------------------------------------- /tests/model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | define_test(model) 7 | target_sources(model PRIVATE main.cpp) 8 | target_sources(model PRIVATE dc_motor.cpp) 9 | -------------------------------------------------------------------------------- /tests/model/dc_motor.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | extern "C" { 12 | #include "control/dynamics.h" 13 | #include "control/model/dc_motor.h" 14 | }; 15 | 16 | void step_for(struct model_dc_motor *m, unsigned int steps) 17 | { 18 | for (unsigned int i = 0; i < steps; i++) { 19 | model_dc_motor_step(m); 20 | } 21 | } 22 | 23 | TEST(DCMotorTest, MotorSpinup) 24 | { 25 | struct model_dc_motor m; 26 | model_dc_motor_init(&m); 27 | m.b = 0.0; 28 | m.u[0] = 0; 29 | m.K = 0.123; 30 | step_for(&m, 20); 31 | m.u[0] = 1; 32 | step_for(&m, 2000); 33 | // with no friction, motor angular velocity will converge to u/K 34 | ASSERT_NEAR(m.u[0], model_dc_motor_get_omega(&m) * m.K, 0.01); 35 | } 36 | -------------------------------------------------------------------------------- /tests/model/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/motion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | define_test(motion) 7 | target_sources(motion PRIVATE main.cpp) 8 | target_sources(motion PRIVATE motion_profile.cpp) 9 | -------------------------------------------------------------------------------- /tests/motion/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/motor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | define_test(motor) 7 | target_sources(motor PRIVATE main.cpp) 8 | target_sources(motor PRIVATE park.cpp) 9 | target_sources(motor PRIVATE clarke.cpp) 10 | -------------------------------------------------------------------------------- /tests/motor/clarke.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | extern "C" { 12 | #include 13 | #include "control/motor.h" 14 | }; 15 | 16 | TEST(ClarkeTransform, Forward) 17 | { 18 | for (float angle = 0; angle < 2 * M_PI; angle += M_PI / 18.f) { 19 | float abc[3] = { cosf(angle), cosf(angle - 2.f * M_PI / 3.f), 20 | cosf(angle + 2.f * M_PI / 3.f) }; 21 | float xyz[3] = { 0 }; 22 | 23 | clarke(xyz, abc); 24 | 25 | // the vectors should be the same length 26 | float n_abc = sqrtf(abc[0] * abc[0] + abc[1] * abc[1] + abc[2] * abc[2]); 27 | float n_xyz = sqrtf(xyz[0] * xyz[0] + xyz[1] * xyz[1] + xyz[2] * xyz[2]); 28 | EXPECT_NEAR(n_abc, n_xyz, 1e-6); 29 | } 30 | } 31 | 32 | TEST(ClarkeTransform, Backward) 33 | { 34 | for (float angle = 0; angle < 2 * M_PI; angle += M_PI / 18.f) { 35 | float abc[3] = { cosf(angle), cosf(angle - 2.f * M_PI / 3.f), 36 | cosf(angle - 4.f * M_PI / 3.f) }; 37 | float xyz[3] = { 0 }; 38 | float abc_n[3] = { 0 }; 39 | clarke(xyz, abc); 40 | inv_clarke(abc_n, xyz); 41 | EXPECT_NEAR(abc[0], abc_n[0], 1e-6); 42 | EXPECT_NEAR(abc[1], abc_n[1], 1e-6); 43 | EXPECT_NEAR(abc[2], abc_n[2], 1e-6); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /tests/motor/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/motor/park.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | extern "C" { 12 | #include 13 | #include "control/motor.h" 14 | }; 15 | 16 | TEST(ParkTransform, Forward) 17 | { 18 | for (float angle = 0; angle < 2 * M_PI; angle += M_PI / 18.f) { 19 | float abc[3] = { cosf(angle), cosf(angle - 2.f * M_PI / 3.f), 20 | cosf(angle + 2.f * M_PI / 3.f) }; 21 | float xyz[3] = { 0 }; 22 | float dqz[3] = { 0 }; 23 | 24 | clarke(xyz, abc); 25 | park(dqz, xyz, angle); 26 | 27 | // the vectors should be the same length 28 | float n_abc = sqrtf(abc[0] * abc[0] + abc[1] * abc[1] + abc[2] * abc[2]); 29 | float n_xyz = sqrtf(dqz[0] * dqz[0] + dqz[1] * dqz[1] + dqz[2] * dqz[2]); 30 | EXPECT_NEAR(n_abc, n_xyz, 1e-6); 31 | } 32 | } 33 | 34 | TEST(ParkTransform, Reverse) 35 | { 36 | for (float angle = 0; angle < 2 * M_PI; angle += M_PI / 18.f) { 37 | float abc[3] = { cosf(angle), cosf(angle - 2.f * M_PI / 3.f), 38 | cosf(angle + 2.f * M_PI / 3.f) }; 39 | float xyz[3] = { 0 }; 40 | float dqz[3] = { 0 }; 41 | float i_xyz[3] = { 0 }; 42 | float i_abc[3] = { 0 }; 43 | 44 | clarke(xyz, abc); 45 | park(dqz, xyz, angle); 46 | inv_park(i_xyz, dqz, angle); 47 | inv_clarke(i_abc, i_xyz); 48 | 49 | EXPECT_NEAR(abc[0], i_abc[0], 1e-6); 50 | EXPECT_NEAR(abc[1], i_abc[1], 1e-6); 51 | EXPECT_NEAR(abc[2], i_abc[2], 1e-6); 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /tests/optimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: MIT 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | define_test(optimize) 7 | target_sources(optimize PRIVATE main.cpp) 8 | target_sources(optimize PRIVATE linprog.cpp) 9 | -------------------------------------------------------------------------------- /tests/optimization/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /tests/sysid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: MIT 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/go 4 | # Training: https://swedishembedded.com/tag/training 5 | 6 | define_test(sysid) 7 | target_sources(sysid PRIVATE main.cpp) 8 | target_sources(sysid PRIVATE okid_era.cpp) 9 | target_sources(sysid PRIVATE rls.cpp) 10 | target_sources(sysid PRIVATE sqr_ukf_id.cpp) 11 | -------------------------------------------------------------------------------- /tests/sysid/main.cpp: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: MIT */ 2 | /* 3 | * Copyright 2022 Martin Schröder 4 | * Consulting: https://swedishembedded.com/go 5 | * Training: https://swedishembedded.com/tag/training 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | -------------------------------------------------------------------------------- /zephyr/Kconfig: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Copyright (c) 2022 Martin Schröder 3 | # Consulting: https://swedishembedded.com/consulting 4 | # Training: https://swedishembedded.com/training 5 | 6 | config CONTROL 7 | bool "Swedish Embedded control engineering library" 8 | default n 9 | depends on NEWLIB_LIBC || EXTERNAL_LIBC 10 | help 11 | Control engineering algorithm library 12 | -------------------------------------------------------------------------------- /zephyr/module.yml: -------------------------------------------------------------------------------- 1 | build: 2 | cmake: zephyr 3 | kconfig: zephyr/Kconfig 4 | --------------------------------------------------------------------------------