├── .gitignore ├── .travis.yml ├── LICENSE ├── README.md ├── bayes_people_tracker ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config │ ├── detectors.yaml │ └── detectors_withnewparams.yaml ├── include │ └── people_tracker │ │ ├── asso_exception.h │ │ ├── flobot_tracking.h │ │ └── people_tracker.h ├── launch │ └── people_tracker.launch ├── msg │ └── PeopleTracker.msg ├── package.xml └── src │ └── people_tracker │ └── people_tracker.cpp ├── bayestracking ├── CHANGELOG.rst ├── CMakeLists.txt ├── Doxyfile.in ├── LICENCE ├── README.md ├── config │ ├── BayestrackingConfig.cmake.in │ ├── BayestrackingConfigVersion.cmake.in │ └── bayestracking.pc.in ├── examples │ ├── CMakeLists.txt │ └── simple_2d_tracking.cpp ├── include │ └── bayes_tracking │ │ ├── BayesFilter │ │ ├── CIFlt.hpp │ │ ├── SIRFlt.hpp │ │ ├── UDFlt.hpp │ │ ├── allFilters.hpp │ │ ├── bayesException.hpp │ │ ├── bayesFlt.hpp │ │ ├── covFlt.hpp │ │ ├── filters │ │ │ ├── average1.hpp │ │ │ └── indirect.hpp │ │ ├── infFlt.hpp │ │ ├── infRtFlt.hpp │ │ ├── itrFlt.hpp │ │ ├── matSup.hpp │ │ ├── matSupSub.hpp │ │ ├── models.hpp │ │ ├── random.hpp │ │ ├── schemeFlt.hpp │ │ ├── uBLASmatrix.hpp │ │ ├── uLAPACK.hpp │ │ └── unsFlt.hpp │ │ ├── angle.h │ │ ├── associationmatrix.h │ │ ├── ekfilter.h │ │ ├── jacobianmodel.h │ │ ├── jpda.h │ │ ├── models.h │ │ ├── multitracker.h │ │ ├── pfilter.h │ │ ├── synchronizer.h │ │ ├── trackwin.h │ │ └── ukfilter.h ├── package.xml └── src │ └── bayes_tracking │ ├── BayesFilter │ ├── CIFlt.cpp │ ├── SIRFlt.cpp │ ├── UDFlt.cpp │ ├── UdU.cpp │ ├── bayesFlt.cpp │ ├── bayesFltAlg.cpp │ ├── covFlt.cpp │ ├── infFlt.cpp │ ├── infRtFlt.cpp │ ├── itrFlt.cpp │ ├── matSup.cpp │ └── unsFlt.cpp │ ├── associationmatrix.cpp │ ├── ekfilter.cpp │ ├── models.cpp │ ├── pfilter.cpp │ ├── trackwin.cpp │ └── ukfilter.cpp └── object3d_detector ├── CMakeLists.txt ├── README.md ├── config ├── object3d_detector.yaml ├── object3d_detector_ol.yaml └── vlp16.yaml ├── include └── svm.h ├── launch ├── object3d_detector.launch ├── object3d_detector.rviz ├── object3d_detector_ol.launch └── object3d_detector_ol.rviz ├── libsvm ├── README.md ├── grid.py ├── pedestrian.model └── pedestrian.range ├── package.xml └── src ├── object3d_detector.cpp ├── object3d_detector_ol.cpp └── svm.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: xenial 2 | sudo: required 3 | language: generic 4 | cache: apt 5 | 6 | install: 7 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 8 | - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 9 | - sudo apt-get update 10 | - sudo apt-get install ros-kinetic-desktop-full ros-kinetic-people-msgs 11 | - source /opt/ros/kinetic/setup.bash 12 | - pip install catkin_pkg empy 13 | - mkdir -p ~/catkin_ws/src 14 | - cd ~/catkin_ws/ 15 | - catkin_make 16 | - source devel/setup.bash 17 | 18 | script: 19 | - cd ~/catkin_ws/src 20 | - git clone -b single-sensor-ros-kinetic https://github.com/yzrobot/online_learning.git 21 | - cd ~/catkin_ws 22 | - catkin_make 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Online learning for human classification in 3D LiDAR-based tracking # 2 | 3 | [![Build Status](https://travis-ci.org/yzrobot/online_learning.svg?branch=master)](https://travis-ci.org/yzrobot/online_learning) 4 | [![Codacy Badge](https://app.codacy.com/project/badge/Grade/63b189ca851b4a30903e19fef1a10d36)](https://www.codacy.com/gh/yzrobot/online_learning/dashboard?utm_source=github.com&utm_medium=referral&utm_content=yzrobot/online_learning&utm_campaign=Badge_Grade) 5 | [![License: GPL v3](https://img.shields.io/badge/License-GPLv3-blue.svg)](https://www.gnu.org/licenses/gpl-3.0) 6 | 7 | :boom: Please check my Ph.D. student's amazing ongoing work (with very high modularity): [efficient_online_learning](https://github.com/epan-utbm/efficient_online_learning) for autonomous driving! :boom: 8 | 9 | This is a ROS-based online learning framework for human classification in 3D LiDAR scans, taking advantage of robust multi-target tracking to avoid the need for data annotation by a human expert. 10 | Please watch the videos below for more details. 11 | 12 | [![YouTube Video 1](https://img.youtube.com/vi/bjztHV9rC-0/0.jpg)](https://www.youtube.com/watch?v=bjztHV9rC-0) 13 | [![YouTube Video 2](https://img.youtube.com/vi/rmPn7mWssto/0.jpg)](https://www.youtube.com/watch?v=rmPn7mWssto) 14 | 15 | For a standalone implementation of the clustering method, please refer to: [https://github.com/yzrobot/adaptive_clustering](https://github.com/yzrobot/adaptive_clustering) 16 | 17 | # Install & Build 18 | ```sh 19 | cd catkin_ws/src 20 | // Install prerequisite packages 21 | git clone https://github.com/wg-perception/people.git 22 | git clone https://github.com/DLu/wu_ros_tools.git 23 | sudo apt-get install ros-kinetic-bfl 24 | // The core 25 | git clone https://github.com/yzrobot/online_learning 26 | // Build 27 | cd catkin_ws 28 | catkin_make 29 | ``` 30 | 31 | # Run 32 | After catkin_make succeed, modify 'line 3' of online_learning/object3d_detector/launch/object3d_detector.launch, and make the value is the path where your bag files are located: 33 | 34 | `` 35 | 36 | The bag file offered by [Lincoln Centre for Autonomous Systems](http://lcas.lincoln.ac.uk) is in velodyne_msgs/VelodyneScan message type, so we would need related velodyne packages in ROS: 37 | ```bash 38 | $ sudo apt-get install ros-kinetic-velodyne* 39 | ``` 40 | Now, the svm should be able to run: 41 | ```bash 42 | $ cd catkin_ws 43 | $ source devel/setup.bash 44 | $ roslaunch object3d_detector object3d_detector.launch 45 | ``` 46 | 47 | ## Citation ## 48 | If you are considering using this code, please reference the following: 49 | 50 | ``` 51 | @inproceedings{yz17iros, 52 | author = {Zhi Yan and Tom Duckett and Nicola Bellotto}, 53 | title = {Online learning for human classification in {3D LiDAR-based} tracking}, 54 | booktitle = {Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 55 | pages = {864--871}, 56 | address = {Vancouver, Canada}, 57 | month = {September}, 58 | year = {2017} 59 | } 60 | ``` 61 | -------------------------------------------------------------------------------- /bayes_people_tracker/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package bayes_people_tracker 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.8 (2015-09-03) 6 | ------------------ 7 | 8 | 1.1.6 (2015-06-24) 9 | ------------------ 10 | * 1.1.5 11 | * updated changelogs 12 | * 1.1.4 13 | * updated changelogs 14 | * Contributors: Jenkins 15 | 16 | 1.1.5 (2015-05-22) 17 | ------------------ 18 | 19 | 1.1.4 (2015-05-10) 20 | ------------------ 21 | 22 | 1.1.3 (2015-04-10) 23 | ------------------ 24 | 25 | 1.1.2 (2015-04-07) 26 | ------------------ 27 | 28 | 1.1.1 (2015-04-03) 29 | ------------------ 30 | 31 | 1.0.0 (2015-03-10) 32 | ------------------ 33 | * Nicer print 34 | * Adding ability to switch between Extended and Unscented Kalman Filter 35 | * Making simple_tracking template based. 36 | * Changed config file structure and made necessary changes to the code. 37 | * Adding pose, pose_array and people publishers to connection callback. 38 | * * Publishing a pose array for all detected people to have more generic output 39 | * Added missing bayes tracker parameters to launch files and READMEs 40 | * Starting the mdl tracker is now optional when using the robot launch file. `with_mdl_tracker=true` starts the mdl tracker in addition to the bayes tracker. Default is `false` 41 | * forgot (again) to change default detector.yaml in bayes_people_tracker 42 | * adding visualization to rviz via nav_msgs/Path 43 | * Contributors: Christian Dondrup, Ferdian Jovan 44 | 45 | 0.1.4 (2015-03-06) 46 | ------------------ 47 | * Publishin people_msgs/People and adding orientation. 48 | * forgot to undo my config for detectors.yaml in bayes_people_tracker 49 | * provide online stitching poses into trajectories 50 | * add online trajectory construction from /people_tracker/positions 51 | * Contributors: Christian Dondrup, Ferdian Jovan 52 | 53 | 0.1.3 (2015-02-25) 54 | ------------------ 55 | 56 | 0.1.1 (2015-02-18) 57 | ------------------ 58 | 59 | 0.1.0 (2015-02-18) 60 | ------------------ 61 | * Setting correct version number. The changelogs will be regenerated because the ones from the release branch would not be consistent with the changes made in the devel branch. 62 | * Small bug in ros_debug statment 63 | * Changed launch files to new format. 64 | * Changed launch files to new format. 65 | * Contributors: Christian Dondrup 66 | 67 | 0.0.14 (2014-11-23) 68 | ------------------- 69 | * Updating changelogs and adjusting version numbers 70 | * 0.0.12 71 | * Adjusting version number. 72 | * Updated changelogs 73 | * 0.0.3 74 | * Updated changelogs 75 | * 0.0.2 76 | * Updated changelog 77 | * 0.0.1 78 | * Created changelogs 79 | * Contributors: Christian Dondrup 80 | 81 | 0.0.13 (2014-10-31 16:14) 82 | ------------------------- 83 | * Updating changelogs and manually bumping version number. 84 | * 0.0.11 85 | * Updated changelogs 86 | * 0.0.10 87 | * Updating changelog 88 | * 0.0.9 89 | * Updated changelogs 90 | * 0.0.8 91 | * Updated changelogs 92 | * 0.0.7 93 | * Updated changelogs 94 | * 0.0.6 95 | * Updated changelogs 96 | * 0.0.5 97 | * Updated changelogs 98 | * 0.0.4 99 | * Updating changelogs 100 | * Removing the leg_detector from the run_dependencies of the launch package for indigo release. 101 | leg_detector is not released for indigo yet. 102 | * 0.0.3 103 | * Updated changelogs 104 | * 0.0.2 105 | * Updated changelog 106 | * 0.0.1 107 | * Created changelogs 108 | * 0.0.11 109 | * Updated changelogs 110 | * 0.0.10 111 | * Updating changelog 112 | * 0.0.9 113 | * Updated changelogs 114 | * 0.0.8 115 | * Updated changelogs 116 | * 0.0.7 117 | * Updated changelogs 118 | * 0.0.6 119 | * Updated changelogs 120 | * 0.0.5 121 | * Updated changelogs 122 | * 0.0.4 123 | * Updating changelogs 124 | * Removing the leg_detector from the run_dependencies of the launch package for indigo release. 125 | leg_detector is not released for indigo yet. 126 | * Contributors: Christian Dondrup 127 | 128 | 0.0.12 (2014-10-31 16:07) 129 | ------------------------- 130 | * Adjusting version number. 131 | * Updated changelogs 132 | * 0.0.3 133 | * Updated changelogs 134 | * 0.0.2 135 | * Updated changelog 136 | * 0.0.1 137 | * Created changelogs 138 | * Added proper link to paper describing bayes_tracker 139 | * Contributors: Christian Dondrup 140 | 141 | 0.0.11 (2014-10-30 11:18) 142 | ------------------------- 143 | * Updated changelogs 144 | * Contributors: Christian Dondrup 145 | 146 | 0.0.10 (2014-10-30 10:19) 147 | ------------------------- 148 | * Updating changelog 149 | * Contributors: Christian Dondrup 150 | 151 | 0.0.9 (2014-10-30 09:52) 152 | ------------------------ 153 | * Updated changelogs 154 | * Contributors: Christian Dondrup 155 | 156 | 0.0.8 (2014-10-30 09:32) 157 | ------------------------ 158 | * Updated changelogs 159 | * Contributors: Christian Dondrup 160 | 161 | 0.0.7 (2014-10-29 20:40) 162 | ------------------------ 163 | * Updated changelogs 164 | * Contributors: Christian Dondrup 165 | 166 | 0.0.6 (2014-10-29 20:32) 167 | ------------------------ 168 | * Updated changelogs 169 | * Contributors: Christian Dondrup 170 | 171 | 0.0.5 (2014-10-29 18:30) 172 | ------------------------ 173 | * Updated changelogs 174 | * Contributors: Christian Dondrup 175 | 176 | 0.0.4 (2014-10-29 18:22) 177 | ------------------------ 178 | * Updating changelogs 179 | * Removing the leg_detector from the run_dependencies of the launch package for indigo release. 180 | leg_detector is not released for indigo yet. 181 | * Contributors: Christian Dondrup 182 | 183 | 0.0.3 (2014-10-23) 184 | ------------------ 185 | * Updated changelogs 186 | * Added LICENSE files. Fixes `#101 `_ 187 | * Contributors: Christian Dondrup, Lucas Beyer 188 | 189 | 0.0.2 (2014-10-18 17:39) 190 | ------------------------ 191 | * Updated changelog 192 | * Contributors: Christian Dondrup 193 | 194 | 0.0.1 (2014-10-18 17:28) 195 | ------------------------ 196 | * Created changelogs 197 | * Renamed strands_pedestrian_tracking to mdl_people_tracker 198 | This also includes renaming the messages and most of the parameters. 199 | * Forgot to install the config dir. 200 | * Fixed missing things 201 | * Prepared bayes_people_tracker for release. 202 | * Splitting utils package into seperate packages. 203 | * Renamed strands_people_tracker to bayes_people_tracker 204 | * Contributors: Christian Dondrup 205 | -------------------------------------------------------------------------------- /bayes_people_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bayes_people_tracker) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | bayes_tracking 9 | geometry_msgs 10 | message_generation 11 | people_msgs 12 | roscpp 13 | std_msgs 14 | tf 15 | visualization_msgs 16 | ) 17 | find_package(Boost REQUIRED COMPONENTS thread) 18 | 19 | ####################################### 20 | ## Declare ROS messages and services ## 21 | ####################################### 22 | 23 | add_message_files( 24 | FILES 25 | PeopleTracker.msg 26 | ) 27 | 28 | generate_messages( 29 | DEPENDENCIES 30 | std_msgs 31 | geometry_msgs 32 | ) 33 | 34 | ################################### 35 | ## catkin specific configuration ## 36 | ################################### 37 | catkin_package( 38 | INCLUDE_DIRS include 39 | CATKIN_DEPENDS bayes_tracking geometry_msgs roscpp std_msgs tf visualization_msgs 40 | ) 41 | 42 | ########### 43 | ## Build ## 44 | ########### 45 | 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${Boost_INCLUDE_DIRS} 50 | ) 51 | 52 | ### Offline version 53 | add_executable(bayes_people_tracker 54 | src/people_tracker/people_tracker.cpp 55 | ) 56 | 57 | add_dependencies(bayes_people_tracker ${PROJECT_NAME}_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) 58 | 59 | target_link_libraries(bayes_people_tracker 60 | ${catkin_LIBRARIES} 61 | ${Boost_LIBRARIES} 62 | ) 63 | 64 | ### Online version 65 | #add_definitions(-DONLINE_LEARNING) 66 | 67 | add_executable(bayes_people_tracker_ol 68 | src/people_tracker/people_tracker.cpp 69 | ) 70 | 71 | add_dependencies(bayes_people_tracker_ol ${PROJECT_NAME}_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) 72 | 73 | target_link_libraries(bayes_people_tracker_ol 74 | ${catkin_LIBRARIES} 75 | ${Boost_LIBRARIES} 76 | ) 77 | 78 | target_compile_definitions(bayes_people_tracker_ol PUBLIC ONLINE_LEARNING=1) 79 | 80 | ############# 81 | ## Install ## 82 | ############# 83 | 84 | install(TARGETS bayes_people_tracker 85 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 88 | ) 89 | 90 | install(TARGETS bayes_people_tracker_ol 91 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 94 | ) 95 | 96 | install(DIRECTORY include/${PROJECT_NAME}/ 97 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 98 | FILES_MATCHING PATTERN "*.h" 99 | PATTERN ".git" EXCLUDE 100 | ) 101 | 102 | install(DIRECTORY launch 103 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 104 | ) 105 | 106 | install(DIRECTORY config 107 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 108 | ) 109 | -------------------------------------------------------------------------------- /bayes_people_tracker/LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 Christian Dondrup 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /bayes_people_tracker/README.md: -------------------------------------------------------------------------------- 1 | ## People Tracker 2 | This package uses the bayes_tracking library developed by Nicola Bellotto (University of Lincoln), please cite with: [10.5281/zenodo.15825](https://zenodo.org/record/15825) and [1] 3 | 4 | The people_tracker uses a single config file to add an arbitrary amount of detectors. The file `config/detectors.yaml` contains the necessary information for the upper_body_detector and the ROS leg_detector (see `to_pose_array` in detector_msg_to_pose_array/README.md): 5 | 6 | ``` 7 | bayes_people_tracker: 8 | filter_type: "UKF" # The Kalman filter type: EKF = Extended Kalman Filter, UKF = Uncented Kalman Filter 9 | cv_noise_params: # The noise for the constant velocity prediction model 10 | x: 1.4 11 | y: 1.4 12 | detectors: # Add detectors under this namespace 13 | upper_body_detector: # Name of detector (used internally to identify them). Has to be unique. 14 | topic: "/upper_body_detector/bounding_box_centres" # The topic on which the geometry_msgs/PoseArray is published 15 | cartesian_noise_params: # The noise for the cartesian observation model 16 | x: 0.5 17 | y: 0.5 18 | matching_algorithm: "NNJPDA" # The algorthim to match different detections. NN = Nearest Neighbour, NNJPDA = NN Joint Probability Data Association 19 | leg_detector: # Name of detector (used internally to identify them). Has to be unique. 20 | topic: "/to_pose_array/leg_detector" # The topic on which the geometry_msgs/PoseArray is published 21 | cartesian_noise_params: # The noise for the cartesian observation model 22 | x: 0.2 23 | y: 0.2 24 | matching_algorithm: "NNJPDA" # The algorthim to match different detections. NN = Nearest Neighbour, NNJPDA = NN Joint Probability Data Association 25 | ``` 26 | 27 | New detectors are added under the parameter namespace `bayes_people_tracker/detectors`. Let's have a look at the upper body detector as an example: 28 | 29 | ### Tracker Parameters 30 | 31 | The tracker offers two configuration parameters: 32 | * `filter_type`: This specefies which variant of the Kalman filter to use. Currently, it implements an Extended and an Unscented Kalman filter which can be chosen via `EKF` and `UKF`, respectively. 33 | * `cv_noise_params`: parameter is used for the constant velocity prediction model. 34 | * specifies the standard deviation of the x and y velocity. 35 | 36 | ### Detector Parameters 37 | 38 | * For every detector you have to create a new namespace where the name is used as an internal identifier for this detector. Therefore it has to be unique. In this case it is `upper_body_detector` 39 | * The `topic` parameter specifies the topic under which the detections are published. The type has to be `geometry_msgs/PoseArray`. See `to_pose_array` in detector_msg_to_pose_array/README.md if your detector does not publish a PoseArray. 40 | * The `cartesian_noise_params` parameter is used for the Cartesian observation model. 41 | * specifies the standard deviation of x and y. 42 | * `matching_algorithm` specifies the algorithm used to match detections from different sensors/detectors. Currently there are two different algorithms which are based on the Mahalanobis distance of the detections (default being NNJPDA if parameter is misspelled): 43 | * NN: Nearest Neighbour 44 | * NNJPDA: Nearest Neighbour Joint Probability Data Association 45 | 46 | All of these are just normal ROS parameters and can be either specified by the parameter server or using the yaml file in the provided launch file. 47 | 48 | ### Message Type: 49 | 50 | The tracker publishes the following: 51 | * `pose`: A `geometry_msgs/PoseStamped` for the clostes person. 52 | * `pose_array`: A `geometry_msgs/PoseArray` for all detections. 53 | * `people`: A `people_msgs/People` for all detections. Can be used for layerd costmaps. 54 | * `marker_array`: A `visualization_msgs/MarkerArray` showing little stick figures for every detection. Figures are orient according to the direction of velocity. 55 | * `trajectory`: A `geometry_msgs/PoseArray` for human trajectory. 56 | * `positions`: A `bayes_people_tracker/PeopleTracker` message. See below. 57 | 58 | ``` 59 | std_msgs/Header header 60 | string[] uuids # Unique uuid5 (NAMESPACE_DNS) person id as string. Id is based on system time on start-up and tracker id. Array index matches ids array index 61 | geometry_msgs/Pose[] poses # The real world poses of the detected people in the given target frame. Default: /map. Array index matches ids/uuids array index 62 | float64[] distances # The distances of the detected persons to the robot (polar coordinates). Array index matches ids array index. 63 | float64[] angles # Angles of the detected persons to the coordinate frames z axis (polar coordinates). Array index matches ids array index. 64 | float64 min_distance # The minimal distance in the distances array. 65 | float64 min_distance_angle # The angle according to the minimal distance. 66 | ``` 67 | 68 | The poses will be published in a given `target_frame` (see below) but the distances and angles will always be relative to the robot in the `/base_link` tf frame. 69 | 70 | ### Running 71 | Parameters: 72 | 73 | * `target_frame`: _Default: /base_link_:the tf frame in which the tracked poses will be published. 74 | * `position`: _Default: /people_tracker/positions_: The topic under which the results are published as bayes_people_tracker/PeopleTracker` 75 | * `pose`: _Default: /people_tracker/pose_: The topic under which the closest detected person is published as a geometry_msgs/PoseStamped` 76 | * `pose_array`: _Default: /people_tracker/pose_array_: The topic under which the detections are published as a geometry_msgs/PoseArray` 77 | * `poeple`: _Default: /people_tracker/people_: The topic under which the results are published as people_msgs/People` 78 | * `marker`: _Default /people_tracker/marker_array_: A visualisation marker array. 79 | 80 | You can run the node with: 81 | 82 | ``` 83 | roslaunch bayes_people_tracker people_tracker.launch 84 | ``` 85 | 86 | This is the recommended way of launching it since this will also read the config file and set the right parameters for the detectors. 87 | 88 | [1] N. Bellotto and H. Hu, “Computationally efficient solutions for tracking people with a mobile robot: an experimental evaluation of bayesian filters,” Autonomous Robots, vol. 28, no. 4, pp. 425–438, 2010. 89 | -------------------------------------------------------------------------------- /bayes_people_tracker/config/detectors.yaml: -------------------------------------------------------------------------------- 1 | bayes_people_tracker: 2 | filter_type: "UKF" # The Kalman filter type: EKF = Extended Kalman Filter, UKF = Uncented Kalman Filter 3 | cv_noise_params: # The noise for the constant velocity prediction model 4 | x: 1.4 5 | y: 1.4 6 | detectors: # Add detectors under this namespace 7 | upper_body_detector: # Name of detector (used internally to identify them). Has to be unique. 8 | topic: "/upper_body_detector/bounding_box_centres" # The topic on which the geometry_msgs/PoseArray is published 9 | cartesian_noise_params: # The noise for the cartesian observation model 10 | x: 0.5 11 | y: 0.5 12 | matching_algorithm: "NNJPDA" # The algorthim to match different detections. NN = Nearest Neighbour, NNJPDA = NN Joint Probability Data Association 13 | leg_detector: # Name of detector (used internally to identify them). Has to be unique. 14 | topic: "/to_pose_array/leg_detector" # The topic on which the geometry_msgs/PoseArray is published 15 | cartesian_noise_params: # The noise for the cartesian observation model 16 | x: 0.2 17 | y: 0.2 18 | matching_algorithm: "NNJPDA" # The algorthim to match different detections. NN = Nearest Neighbour, NNJPDA = NN Joint Probability Data Association 19 | -------------------------------------------------------------------------------- /bayes_people_tracker/config/detectors_withnewparams.yaml: -------------------------------------------------------------------------------- 1 | bayes_people_tracker: 2 | filter_type: "UKF" # The Kalman filter type: EKF = Extended Kalman Filter, UKF = Uncented Kalman Filter 3 | cv_noise_params: # The noise for the constant velocity prediction model 4 | x: 1.4 5 | y: 1.4 6 | std_limit: 1.0 # upper limit for the standard deviation of the estimated position 7 | detectors: # Add detectors under this namespace 8 | upper_body_detector: # Name of detector (used internally to identify them). Has to be unique. 9 | topic: "/upper_body_detector/bounding_box_centres" # The topic on which the geometry_msgs/PoseArray is published 10 | cartesian_noise_params: # The noise (standard deviation) for the cartesian observation model 11 | x: 0.5 12 | y: 0.5 13 | matching_algorithm: "NNJPDA" # The algorthim to match different detections. NN = Nearest Neighbour, NNJPDA = NN Joint Probability Data Association 14 | seq_size: 5 # Minimum number of observations for new track creation 15 | seq_time: 0.2 # Minimum interval between observations for new track creation 16 | leg_detector: # Name of detector (used internally to identify them). Has to be unique. 17 | topic: "/to_pose_array/leg_detector" # The topic on which the geometry_msgs/PoseArray is published 18 | cartesian_noise_params: # The noise for the cartesian observation model 19 | x: 0.2 20 | y: 0.2 21 | matching_algorithm: "NNJPDA" # The algorthim to match different detections. NN = Nearest Neighbour, NNJPDA = NN Joint Probability Data Association 22 | seq_size: 4 # Minimum number of observations for new track creation 23 | seq_time: 0.3 # Minimum interval between observations for new track creation 24 | -------------------------------------------------------------------------------- /bayes_people_tracker/include/people_tracker/asso_exception.h: -------------------------------------------------------------------------------- 1 | #ifndef ASSO_EXCEPTION_H 2 | #define ASSO_EXCEPTION_H 3 | 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | struct asso_exception: public exception 10 | { 11 | const char* what() const throw() 12 | { 13 | return "Unknown association algorithm!"; 14 | } 15 | }; 16 | 17 | struct observ_exception: public exception 18 | { 19 | const char* what() const throw() 20 | { 21 | return "Unknown observation model!"; 22 | } 23 | }; 24 | 25 | struct filter_exception: public exception 26 | { 27 | const char* what() const throw() 28 | { 29 | return "Unknown filter type!"; 30 | } 31 | }; 32 | 33 | #endif // ASSO_EXCEPTION_H 34 | -------------------------------------------------------------------------------- /bayes_people_tracker/launch/people_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /bayes_people_tracker/msg/PeopleTracker.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | string[] uuids # Unique uuid5 (NAMESPACE_DNS) person id as string. Id is based on system time on start-up and tracker id. Array index matches ids array index 3 | geometry_msgs/Pose[] poses # The real world poses of the detected people in the given target frame. Default: /map. Array index matches ids/uuids array index 4 | float64[] distances # The distances of the detected persons to the robot (polar coordinates). Array index matches ids array index. 5 | float64[] angles # Angles of the detected persons to the coordinate frames z axis (polar coordinates). Array index matches ids array index. 6 | float64 min_distance # The minimal distance in the distances array. 7 | float64 min_distance_angle # The angle according to the minimal distance. 8 | -------------------------------------------------------------------------------- /bayes_people_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bayes_people_tracker 4 | 1.1.8 5 | The bayes_people_tracker package 6 | 7 | Christian Dondrup 8 | Zhi Yan 9 | 10 | MIT 11 | 12 | https://github.com/strands-project/strands_perception_people 13 | 14 | Christian Dondrup 15 | 16 | catkin 17 | 18 | boost 19 | bayes_tracking 20 | geometry_msgs 21 | message_generation 22 | people_msgs 23 | roscpp 24 | std_msgs 25 | tf 26 | visualization_msgs 27 | nav_msgs 28 | message_filters 29 | 30 | boost 31 | bayes_tracking 32 | geometry_msgs 33 | message_runtime 34 | people_msgs 35 | roscpp 36 | std_msgs 37 | tf 38 | visualization_msgs 39 | nav_msgs 40 | message_filters 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /bayestracking/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package bayes_tracking 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.2 (2014-09-03) 6 | ------------------ 7 | * Adding boost as a build an run dependency. 8 | * Changing Licence to GPL 9 | * Contributors: Christian Dondrup 10 | 11 | 1.0.5 (2014-10-29) 12 | ------------------ 13 | * Merge pull request `#9 `_ from LCAS/indigo-fix 14 | Trusty/Indigo fix 15 | * Trusty/Indigo fix 16 | * Contributors: Christian Dondrup 17 | 18 | 1.0.4 (2014-10-29) 19 | ------------------ 20 | * Merge pull request `#8 `_ from LCAS/opencv 21 | Changing dependency from opencv2 to cv_bridge for indigo release. 22 | * Changing dependency from opencv2 to cv_bridge for indigo release. 23 | * Contributors: Christian Dondrup 24 | 25 | 1.0.3 (2014-09-09) 26 | ------------------ 27 | * "Fixing" the missing JPDA 28 | The option of using only JPDA as an association algorithm was a relic from past development and is not supported anymore. 29 | This change just comments the JPDA in the enum, preventing people from trying to use it. 30 | * Using the correct licence now. 31 | * 1.0.2 32 | * Updated changelog for 1.0.2 33 | * Adding boost as a build an run dependency. 34 | * 1.0.1 35 | * Reverting the version number increase. 36 | * Merge pull request `#5 `_ from cdondrup/master 37 | Release preparations 38 | * Contributors: Christian Dondrup 39 | 40 | 1.0.1 (2014-09-02) 41 | ------------------ 42 | * Merge pull request `#4 `_ from cdondrup/master 43 | Renamed `bayestracking` to `bayes_tracking` to comply with ros naming conventions. 44 | * Renamed `bayestracking` to `bayes_tracking` to comply with ros naming conventions. 45 | * Merge pull request `#2 `_ from cdondrup/master 46 | Adding optional catkin build 47 | * * Enabling normal cmake build if catkin is not available. 48 | * Updated package.xml and create install targets for ROS release 49 | * Refactored README to markdown 50 | * Refactored bayestracking library to be a catkin package 51 | Created a cleaner file structure by seperating the src from the include files. 52 | * Update README 53 | * Update README 54 | * Now generates cmake config files to enable usage of find_package 55 | * Added the creation of a pkg-config file to make the library easier to use. 56 | * PFilter not working right now in simple_tracking.cpp, but all other tested filters (UKF and EKF) worked very well 57 | * fixed size_t uint issue 58 | * compiles now (but simple_tracking example still gives errors) 59 | * initial import form package 60 | * Contributors: Christian Dondrup, Marc Hanheide 61 | -------------------------------------------------------------------------------- /bayestracking/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bayes_tracking) 3 | set(CPACK_PACKAGE_NAME "bayes_tracking") 4 | set(VERSION "1.0.3") 5 | 6 | ## Find catkin macros and libraries if installed 7 | find_package(catkin QUIET) 8 | ## Use catkin macros and include dirs 9 | if(catkin_FOUND) 10 | message(STATUS "Found catkin") 11 | catkin_package( 12 | INCLUDE_DIRS include 13 | LIBRARIES ${PROJECT_NAME} 14 | ) 15 | include_directories( 16 | ${catkin_INCLUDE_DIRS} 17 | ) 18 | else(catkin_FOUND) 19 | message(STATUS "NOT found catkin") 20 | endif(catkin_FOUND) 21 | 22 | ## Headers 23 | include_directories( 24 | include 25 | ) 26 | ## Source files 27 | add_library(${PROJECT_NAME} STATIC 28 | src/bayes_tracking/associationmatrix.cpp 29 | src/bayes_tracking/ekfilter.cpp 30 | src/bayes_tracking/ukfilter.cpp 31 | src/bayes_tracking/pfilter.cpp 32 | ## src/bayes_tracking/trackwin.cpp 33 | src/bayes_tracking/models.cpp 34 | src/bayes_tracking/BayesFilter/bayesFltAlg.cpp 35 | src/bayes_tracking/BayesFilter/bayesFlt.cpp 36 | src/bayes_tracking/BayesFilter/CIFlt.cpp 37 | src/bayes_tracking/BayesFilter/covFlt.cpp 38 | src/bayes_tracking/BayesFilter/infFlt.cpp 39 | src/bayes_tracking/BayesFilter/infRtFlt.cpp 40 | src/bayes_tracking/BayesFilter/itrFlt.cpp 41 | src/bayes_tracking/BayesFilter/matSup.cpp 42 | src/bayes_tracking/BayesFilter/SIRFlt.cpp 43 | src/bayes_tracking/BayesFilter/UDFlt.cpp 44 | src/bayes_tracking/BayesFilter/UdU.cpp 45 | src/bayes_tracking/BayesFilter/unsFlt.cpp 46 | ) 47 | 48 | ## Link catkin libraries and set install targets 49 | if(catkin_FOUND) 50 | target_link_libraries(${PROJECT_NAME} 51 | ${catkin_LIBRARIES} 52 | ) 53 | 54 | install(TARGETS ${PROJECT_NAME} 55 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 58 | ) 59 | 60 | install(DIRECTORY include/${PROJECT_NAME}/ 61 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 62 | PATTERN ".svn" EXCLUDE 63 | ) 64 | endif(catkin_FOUND) 65 | 66 | ## add_subdirectory(examples) 67 | 68 | ## Optional builds: documentation 69 | OPTION (BAYESTRACKING_BUILD_DOC "Generates API documentation" OFF) 70 | if(BAYESTRACKING_BUILD_DOC) 71 | ## Check if doxygen is even installed 72 | find_package(Doxygen) 73 | if (DOXYGEN_FOUND STREQUAL "NO") 74 | message(FATAL_ERROR "Doxygen not found. Please get a copy http://www.doxygen.org") 75 | endif (DOXYGEN_FOUND STREQUAL "NO") 76 | 77 | ## Prepare doxygen configuration file 78 | configure_file(${CMAKE_SOURCE_DIR}/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 79 | 80 | ## Add doxygen as target 81 | add_custom_target(doc ALL ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile 82 | COMMENT "Generating API documentation with Doxygen" VERBATIM) 83 | 84 | ## Cleanup $build/doc on "make clean" 85 | set_property(DIRECTORY APPEND PROPERTY 86 | ADDITIONAL_MAKE_CLEAN_FILES doc) 87 | 88 | ## Install HTML API documentation and manual pages 89 | set(DOC_PATH "share/doc/${CPACK_PACKAGE_NAME}-${VERSION}") 90 | 91 | install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doc/html 92 | DESTINATION ${DOC_PATH} 93 | ) 94 | 95 | endif(BAYESTRACKING_BUILD_DOC) 96 | -------------------------------------------------------------------------------- /bayestracking/Doxyfile.in: -------------------------------------------------------------------------------- 1 | PROJECT_NAME = ${CPACK_PACKAGE_NAME} 2 | PROJECT_NUMBER = ${VERSION} 3 | OUTPUT_DIRECTORY = doc 4 | INPUT = ${CMAKE_CURRENT_SOURCE_DIR}/src 5 | GENERATE_LATEX = NO 6 | QUIET = YES 7 | EXTRACT_ALL = YES 8 | -------------------------------------------------------------------------------- /bayestracking/LICENCE: -------------------------------------------------------------------------------- 1 | This program is free software; you can redistribute it and/or modify 2 | it under the terms of the GNU General Public License as published by 3 | the Free Software Foundation; either version 2 of the License, or 4 | (at your option) any later version. 5 | 6 | This program is distributed in the hope that it will be useful, 7 | but WITHOUT ANY WARRANTY; without even the implied warranty of 8 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 | GNU General Public License for more details. 10 | 11 | You should have received a copy of the GNU General Public License 12 | along with this program; if not, write to the 13 | Free Software Foundation, Inc., 14 | 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 15 | -------------------------------------------------------------------------------- /bayestracking/README.md: -------------------------------------------------------------------------------- 1 | ## BayesTracking - a library for Bayesian tracking 2 | 3 | ### Requirements 4 | * Bayes++ (included in the repository) 5 | * OpenCV 2.x (only to build the examples) 6 | 7 | ### Install (without catkin) 8 | * Extract the library's compressed file 9 | * In the library's directory, create a sub-directory `build` 10 | * Enter in `./build` and run `cmake ..` 11 | * Run `make` to build the library 12 | * Run `make install` (as super user) to install the library and relative header files 13 | 14 | ### Install (ROS version) 15 | * Run `rosdep` to resolve dependencies 16 | * Run `catkin_make` 17 | * Run `catkin_make install` to install the library and headers (optional) 18 | 19 | 20 | _by Nicola Bellotto _ 21 | 22 | *cite with this DOI: [10.5281/zenodo.10318](http://dx.doi.org/10.5281/zenodo.10318)* 23 | -------------------------------------------------------------------------------- /bayestracking/config/BayestrackingConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the bayestracking package 2 | # It defines the following variables 3 | # BAYESTRACKING_INCLUDE_DIRS - include directories for FooBar 4 | # BAYESTRACKING_LIBRARIES - libraries to link against 5 | 6 | # Compute paths 7 | get_filename_component(BAYESTRACKING_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 8 | set(BAYESTRACKING_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") 9 | 10 | # Our library dependencies (contains definitions for IMPORTED targets) 11 | if(NOT TARGET bayestracking AND NOT Bayestracking_BINARY_DIR) 12 | include("${BAYESTRACKING_CMAKE_DIR}/BayestrackingTargets.cmake") 13 | endif() 14 | 15 | # These are IMPORTED targets created by FooBarTargets.cmake 16 | set(BAYESTRACKING_LIBRARIES bayestracking) 17 | -------------------------------------------------------------------------------- /bayestracking/config/BayestrackingConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | set(PACKAGE_VERSION "@VERSION@") 2 | 3 | # Check whether the requested PACKAGE_FIND_VERSION is compatible 4 | if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 5 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 6 | else() 7 | set(PACKAGE_VERSION_COMPATIBLE TRUE) 8 | if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") 9 | set(PACKAGE_VERSION_EXACT TRUE) 10 | endif() 11 | endif() 12 | -------------------------------------------------------------------------------- /bayestracking/config/bayestracking.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=${prefix} 3 | libdir=${exec_prefix}/lib 4 | includedir=${prefix}/include 5 | 6 | Name: bayestracking 7 | Description: BayesTracking - a library for Bayesian tracking 8 | Version: @VERSION@ 9 | Cflags: -I${includedir}/bayestracking 10 | Libs: -L${libdir} -lbayestracking 11 | 12 | -------------------------------------------------------------------------------- /bayestracking/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(../include) 2 | 3 | add_executable(simple_2d_tracking simple_2d_tracking.cpp) 4 | 5 | 6 | find_package( OpenCV REQUIRED ) 7 | 8 | target_link_libraries(simple_2d_tracking bayes_tracking ${OpenCV_LIBS}) 9 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/CIFlt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_CI 2 | #define _BAYES_FILTER_CI 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Covariance Intersection Filter Scheme. 14 | * 15 | * References 16 | * [1] "A Non divergent Estimation Algorithm in the Presence of Unknown Correlations" 17 | * Simon J Julier, Jeffrey K Uhlmann 18 | * 19 | * CI provides a generalised consistent method to combine mean and covariances 20 | * of two estimates. The combination can be optimised by choosing a norm of the 21 | * combined correlations. The norm (omega) is restrict to 0..1 inclusive an effectively 22 | * scales the combination 23 | * Here is CI with a predict and observe model to form a filter. 24 | * 25 | * The Omega norm chosen here is the fixed value of 0.5 26 | * The Omega function should be overloaded to produce more useful results 27 | * 28 | * The filter is operated by performing a 29 | * predict, observe 30 | * cycle derived from the Extended_filter 31 | */ 32 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 33 | 34 | /* Filter namespace */ 35 | namespace Bayesian_filter 36 | { 37 | 38 | class CI_scheme : public Extended_kalman_filter 39 | { 40 | public: 41 | CI_scheme (std::size_t x_size, std::size_t z_initialsize = 0); 42 | CI_scheme& operator= (const CI_scheme&); 43 | // Optimise copy assignment to only copy filter state 44 | 45 | void init (); 46 | void update (); 47 | Float predict (Linrz_predict_model& f); 48 | Float observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s); 49 | Float observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s); 50 | 51 | virtual Float Omega(const FM::SymMatrix& Ai, const FM::SymMatrix& Bi, const FM::SymMatrix& A) 52 | // Determine norm Omega 0..1 for the CI combination 53 | // Default norm is the fixed value 0.5 54 | { 55 | return 0.5; 56 | } 57 | 58 | public: // Exposed Numerical Results 59 | FM::SymMatrix S, SI; // Innovation Covariance and Inverse 60 | 61 | protected: // allow fast operation if z_size remains constant 62 | std::size_t last_z_size; 63 | void observe_size (std::size_t z_size); 64 | }; 65 | 66 | 67 | }//namespace 68 | #endif 69 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/UDFlt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_UD 2 | #define _BAYES_FILTER_UD 3 | /* 4 | * Bayes++ the Bayesian Filtering Library 5 | * Copyright (c) 2002 Michael Stevens 6 | * See accompanying Bayes++.htm for terms and conditions of use. 7 | * 8 | * $Id$ 9 | */ 10 | 11 | /* 12 | * UdU' Factorisation of covariance Filter Scheme. 13 | * Implementation of a 'Square-root' linearised Kalman filter 14 | * 15 | * Bierman's UD factorisatised update algorithm using Agee-Turner UdU' factorisation rank 1 update 16 | * Thornton's MWG-S factorisation predict algorithm 17 | * References 18 | * [1] "Factorisation Methods for Discrete Sequential Estimation" Gerald J. Bierman ISBN 0-12-097350-2 19 | * [2] "Kalman Filtering, Theory and Practice", Mohinder S. Grewal, Angus P. Andrews ISBN 0-13-211335-X 20 | * 21 | * A initial observation size may also be specified for efficiency. 22 | * 23 | * The filter is operated by performing a 24 | * predict, observe 25 | * cycle defined by the base class 26 | */ 27 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 28 | 29 | /* Filter namespace */ 30 | namespace Bayesian_filter 31 | { 32 | 33 | class UD_sequential_observe_model : public Linrz_uncorrelated_observe_model 34 | { 35 | public: 36 | UD_sequential_observe_model (std::size_t x_size, std::size_t z_size) : 37 | Linrz_uncorrelated_observe_model(x_size, z_size), Hx_o(x_size) 38 | {} 39 | virtual const FM::Vec& ho (const FM::Vec& x, const std::size_t o) = 0; 40 | /* Supplied model (h) for observation using state x, z allows normalisation and model variation 41 | Fast model of a single element (o) in observation model 42 | Precondition: Hx_o is conformantly dimensioned 43 | Postcondition: 44 | z(k|k-1) = h(x(k|k-1) 45 | Hx_o(x(k-1|k-1) = Jacobian of h with respect to state x (row o) 46 | */ 47 | FM::Vec Hx_o; 48 | }; 49 | 50 | 51 | class UD_scheme : public Linrz_kalman_filter 52 | { 53 | private: 54 | std::size_t q_max; // Maxiumum size allocated for noise model, constructed before UD 55 | public: 56 | FM::Matrix UD; // UDU factorisation of X with D on diagonal 57 | // Lower triangle used as workspace 58 | FM::Vec s; // Innovation 59 | FM::Vec Sd; // Innovation Covariance 60 | 61 | UD_scheme (std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize = 0); 62 | UD_scheme& operator= (const UD_scheme&); 63 | // Optimise copy assignment to only copy filter state 64 | 65 | void init (); 66 | void update (); 67 | Float predict (Linrz_predict_model& f); 68 | 69 | Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z); 70 | /* No solution for Correlated noise and Linrz model */ 71 | Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z); 72 | /* General observe */ 73 | 74 | Float observe (Linear_correlated_observe_model& h, const FM::Vec& z); 75 | /* Special Linear observe for correlated Z, fast Z decorrelation */ 76 | Float observe (UD_sequential_observe_model& h, const FM::Vec& z); 77 | /* Special Linrz observe using fast sequential model */ 78 | 79 | protected: 80 | Float predictGq (const FM::Matrix& Fx, const FM::Matrix& G, const FM::Vec& q); 81 | FM::Vec d, dv, v; // predictGQ temporaries 82 | Float observeUD (FM::Vec& gain, Float& alpha, const FM::Vec& h, const Float r); 83 | FM::Vec a, b; // observeUD temporaries 84 | // Observation temporaies 85 | void observe_size (std::size_t z_size); 86 | std::size_t last_z_size; 87 | FM::Vec h1; // Single Observation model 88 | FM::Vec w; // Single Gain 89 | FM::Vec znorm; // Normalised innovation 90 | FM::Vec zpdecol; // Decorrelated zp 91 | FM::Matrix Gz; // Z coupling 92 | FM::Matrix GIHx; // Modified Model for linear decorrelation 93 | }; 94 | 95 | 96 | }//namespace 97 | #endif 98 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/allFilters.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _ALL_FILTERS 2 | #define _ALL_FILTERS 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Bayesian Filters. 14 | * A Bayesian filters as an Abstract class. 15 | * All filters have similar structure with vary algorithms and numerical properties 16 | */ 17 | 18 | #include "bayes_tracking/BayesFilter/UDFlt.hpp" 19 | #include "bayes_tracking/BayesFilter/CIFlt.hpp" 20 | #include "bayes_tracking/BayesFilter/unsFlt.hpp" 21 | #include "bayes_tracking/BayesFilter/covFlt.hpp" 22 | #include "bayes_tracking/BayesFilter/infFlt.hpp" 23 | #include "bayes_tracking/BayesFilter/infRtFlt.hpp" 24 | #include "bayes_tracking/BayesFilter/itrFlt.hpp" 25 | #include "bayes_tracking/BayesFilter/SIRFlt.hpp" 26 | #include "bayes_tracking/BayesFilter/models.hpp" 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/bayesException.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_EXCEPTION 2 | #define _BAYES_FILTER_EXCEPTION 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Exception types: Exception hierarchy for Bayesian filtering 14 | */ 15 | 16 | // Common headers required for declerations 17 | #include 18 | 19 | /* Filter namespace */ 20 | namespace Bayesian_filter 21 | { 22 | 23 | 24 | class Filter_exception : virtual public std::exception 25 | /* 26 | * Base class for all exception produced by filter hierarchy 27 | */ 28 | { 29 | public: 30 | const char *what() const throw() 31 | { return error_description; 32 | } 33 | protected: 34 | Filter_exception (const char* description) 35 | { error_description = description; 36 | } 37 | private: 38 | const char* error_description; 39 | }; 40 | 41 | class Logic_exception : virtual public Filter_exception 42 | /* 43 | * Logic Exception 44 | */ 45 | { 46 | public: 47 | Logic_exception (const char* description) : 48 | Filter_exception (description) 49 | {} 50 | }; 51 | 52 | class Numeric_exception : virtual public Filter_exception 53 | /* 54 | * Numeric Exception 55 | */ 56 | { 57 | public: 58 | Numeric_exception (const char* description) : 59 | Filter_exception (description) 60 | {} 61 | }; 62 | 63 | 64 | }//namespace 65 | #endif 66 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/covFlt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_COVARIANCE 2 | #define _BAYES_FILTER_COVARIANCE 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Covariance Filter Scheme. 14 | * Implementation of extended Kalman filter 15 | * 16 | * To work with with Linear and Linrz models 17 | * a) a state separate from covariance predict is used. 18 | * b) a EKF innovation update algorithm is used. 19 | * Discontinuous observe models require that predict is normalised with 20 | * respect to the observation. 21 | * 22 | * A initial observation size may also be specified for efficiency. 23 | * 24 | * The filter is operated by performing a 25 | * predict, observe 26 | * cycle defined by the base class 27 | */ 28 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 29 | 30 | /* Filter namespace */ 31 | namespace Bayesian_filter 32 | { 33 | 34 | class Covariance_scheme : public Extended_kalman_filter 35 | { 36 | public: 37 | Covariance_scheme (std::size_t x_size, std::size_t z_initialsize = 0); 38 | Covariance_scheme& operator= (const Covariance_scheme&); 39 | // Optimise copy assignment to only copy filter state 40 | 41 | void init (); 42 | void update (); 43 | 44 | Float predict (Linrz_predict_model& f); 45 | // Standard Linrz prediction 46 | Float predict (Gaussian_predict_model& f); 47 | // Specialised 'stationary' prediction, only additive noise 48 | 49 | Float observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s); 50 | Float observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s); 51 | 52 | public: // Exposed Numerical Results 53 | FM::SymMatrix S, SI; // Innovation Covariance and Inverse 54 | FM::Matrix W; // Kalman Gain 55 | 56 | protected: // Permanently allocated temps 57 | FM::RowMatrix tempX; 58 | protected: // allow fast operation if z_size remains constant 59 | std::size_t last_z_size; 60 | void observe_size (std::size_t z_size); 61 | }; 62 | 63 | 64 | }//namespace 65 | #endif 66 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/filters/average1.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_AVERAGE1 2 | #define _BAYES_FILTER_AVERAGE1 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Predefined filter: Average1_filter 14 | * A single state averager 15 | */ 16 | 17 | /* Filter namespace */ 18 | namespace Bayesian_filter 19 | { 20 | 21 | template 22 | class Average1_filter 23 | { 24 | Filter_base ksf; // Kalman State Filter 25 | 26 | typedef typename Filter_base::Float Float; 27 | class Cpredict : public Linear_predict_model 28 | // Constant predict model 29 | { 30 | public: 31 | Cpredict(Float qq) : Linear_predict_model(1, 1) 32 | { 33 | Fx(0,0) = 1.; 34 | q[0] = qq; 35 | G(0,0) = 1.; 36 | } 37 | }; 38 | 39 | class Cobserve : public Linear_correlated_observe_model 40 | // Constant observe model 41 | { 42 | public: 43 | Cobserve(Float ZZ) : Linear_correlated_observe_model(1,1) 44 | { 45 | Hx(0,0) = 1.; 46 | Z(0,0) = ZZ; 47 | } 48 | }; 49 | 50 | public: 51 | Average1_filter (Float iQ, Float iZ, Float z); 52 | Average1_filter (Float iQ, Float iZ); 53 | Float observe (Float zz); 54 | operator Float () const 55 | /* Returns filtered estimate 56 | */ 57 | { if (!bInit) 58 | ksf.error (Logic_exception("Average1 not init")); 59 | return ksf.x[0]; 60 | } 61 | 62 | private: 63 | Cpredict f; 64 | Cobserve h; 65 | 66 | bool bInit; 67 | FM::Vec z; 68 | }; 69 | 70 | 71 | 72 | template 73 | Average1_filter::Average1_filter (Float iQ, Float iZ, Float zz) : 74 | ksf(1), f(iQ), h(iZ), z(1) 75 | /* Initialise noises and set sizes 76 | * include first observation zz */ 77 | { 78 | bInit = false; 79 | 80 | observe (zz); 81 | } 82 | 83 | template 84 | Average1_filter::Average1_filter (Float iQ, Float iZ) : 85 | ksf(1), f(iQ), h(iZ), z(1) 86 | // Initialise noises and set sizes 87 | { 88 | bInit = false; 89 | } 90 | 91 | template 92 | typename Average1_filter::Float Average1_filter::observe(Float zz) 93 | /* Observe z, first call set initial state to z 94 | * Returns filtered estimate 95 | */ 96 | { 97 | z[0] = zz; 98 | 99 | if (!bInit) { 100 | ksf.init_kalman (z, h.Z); 101 | bInit = true; 102 | } 103 | 104 | ksf.predict(f); 105 | ksf.observe(h, z); 106 | ksf.update (); 107 | 108 | return ksf.x[0]; 109 | } 110 | 111 | }//namespace 112 | #endif 113 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/filters/indirect.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_INDIRECT 2 | #define _BAYES_FILTER_INDIRECT 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Indirect Filter Adaptor 14 | * Hides the details of the indirect operation of the filter 15 | * The error filter uses the same linear models as the direct filter, 16 | * observation error computation (subtraction) is linear! 17 | */ 18 | 19 | /* Filter namespace */ 20 | namespace Bayesian_filter 21 | { 22 | 23 | 24 | template 25 | class Indirect_state_filter : public State_filter { 26 | /* Indirect state filter 27 | * Estimates state using an associated observation error filter 28 | */ 29 | public: 30 | Indirect_state_filter (Error_base& error_filter) 31 | : State_filter(error_filter.x.size()), direct(error_filter) 32 | { // Construct and zero initial error filter 33 | direct.x.clear(); 34 | } 35 | 36 | template 37 | void predict (P_model& f) 38 | { 39 | x = f.f(x); 40 | direct.predict(f); // May be optimised for linear f as x = 0 41 | }; 42 | 43 | template 44 | void observe (O_model& h, const FM::Vec& z) 45 | { 46 | // Observe error (explicit temporary) 47 | FM::Vec z_error(z.size()); 48 | z_error = h.h(x); 49 | z_error -= z; 50 | direct.observe (h, z_error); 51 | // Update State estimate with error 52 | x -= direct.x; 53 | // Reset the error 54 | direct.x.clear(); 55 | } 56 | 57 | template 58 | void observe_error (O_model& h, const FM::Vec& z_error) 59 | { 60 | direct.observe (h, z_error); 61 | // Update State estimate with error 62 | x -= direct.x; 63 | // Reset the error 64 | direct.x.clear(); 65 | } 66 | 67 | void update () 68 | /* Update filters state 69 | Updates x(k|k) 70 | */ 71 | {} 72 | 73 | private: 74 | Error_base& direct; 75 | }; 76 | 77 | 78 | template 79 | class Indirect_kalman_filter : public Kalman_state_filter { 80 | /* Indirect Kalman filter 81 | * Estimates state using an associated observation error filter 82 | */ 83 | public: 84 | Indirect_kalman_filter (Error_base& error_filter) 85 | : Kalman_state_filter(error_filter.x.size()), direct(error_filter) 86 | { 87 | } 88 | 89 | void init () 90 | /* Initialise from state and state covariance 91 | */ 92 | { 93 | direct.x.clear(); // Zero initial error 94 | direct.X = X; 95 | direct.init(); 96 | } 97 | 98 | template 99 | void predict (P_model& f) 100 | { 101 | x = f.f(x); 102 | direct.predict(f); // May be optimised for linear f as x = 0 103 | }; 104 | 105 | template 106 | void observe (O_model& h, const FM::Vec& z) 107 | { 108 | // Observe error (explicit temporary) 109 | FM::Vec z_error(z.size()); 110 | z_error = h.h(x); 111 | z_error -= z; 112 | direct.observe (h, z_error); 113 | direct.update(); 114 | // Update State estimate with error 115 | x -= direct.x; 116 | // Reset the error 117 | direct.x.clear(); 118 | direct.init (); 119 | } 120 | 121 | template 122 | void observe_error (O_model& h, const FM::Vec& z_error) 123 | { 124 | direct.observe (h, z_error); 125 | // Update State estimate with error 126 | x -= direct.x; 127 | // Reset the error 128 | direct.clear(); 129 | } 130 | 131 | void update () 132 | /* Update filters state 133 | Updates x(k|k) 134 | */ 135 | { 136 | direct.update(); 137 | X = direct.X; 138 | } 139 | 140 | private: 141 | Error_base& direct; 142 | }; 143 | 144 | 145 | }//namespace 146 | #endif 147 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/infFlt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_INFORMATION 2 | #define _BAYES_FILTER_INFORMATION 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Information Filter Scheme. 14 | * A possibly non-linear Information filter 15 | * 16 | * References 17 | * [1] "Stochastic Models, Estimation, and Control} Peter S Maybeck, Academic Press, ISBN 0-12-480701-1 18 | * Section 5.7 19 | * [2] "Kalman Filtering, Theory and Practice", Mohinder S. Grewal, Angus P. Andrews ISBN 0-13-211335-X 20 | * To work with with Linear and Linrz models 21 | * a) a separate state and information (via covariance) predict is used. 22 | * b) a EIF modified innovation update algorithm is used. 23 | * 24 | * Two alternative algorithms are used for predict functions: 25 | * For linrz models an extended predict form is used so information state 'y' is predicted via the 26 | * non-linear function. This requires that X, and Y are invertible so 'x' can be computed. 27 | * For linear invertible models predict can be done directly without computing x 28 | * Discontinuous observe models require that predict is normailised with 29 | * respect to the observation. 30 | * NUMERICS 31 | * The state x is represented by prod(X,y). This may be ill conditioned if the product is 32 | * ill conditioned. At present only the conditioning of X if checked, if y has a large range the product 33 | * may be ill conditioned. This is not checked 34 | * 35 | * The filter is operated by performing a 36 | * predict, observe 37 | * cycle defined by the base class 38 | */ 39 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 40 | 41 | /* Filter namespace */ 42 | namespace Bayesian_filter 43 | { 44 | 45 | class Information_scheme : public Extended_kalman_filter, virtual public Information_state_filter 46 | { 47 | public: 48 | Information_scheme (std::size_t x_size, std::size_t z_initialsize = 0); 49 | Information_scheme& operator= (const Information_scheme&); 50 | // Optimise copy assignment to only copy filter state 51 | 52 | struct Linear_predict_byproducts 53 | { 54 | Linear_predict_byproducts (std::size_t x_size, std::size_t q_size); 55 | FM::SymMatrix A; 56 | FM::Matrix tempG; 57 | FM::SymMatrix B; 58 | FM::Vec y; 59 | }; 60 | 61 | 62 | void init (); 63 | void update (); 64 | void init_yY (); 65 | void update_yY (); 66 | // Covariance and information form state interface 67 | 68 | Float predict (Linear_invertable_predict_model& f, Linear_predict_byproducts& b); 69 | // Linear Prediction in information form as in Ref[2] 70 | Float predict (Linear_invertable_predict_model& f) 71 | { 72 | Linear_predict_byproducts b(f.Fx.size1(),f.q.size()); 73 | return predict (f, b); 74 | } 75 | 76 | Float predict (Linrz_predict_model& f); 77 | // Extended Prediction via state 78 | 79 | Float observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s); 80 | Float observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s); 81 | 82 | protected: 83 | bool update_required; // Postcondition of update is not met 84 | 85 | protected: // Permanently allocated temps 86 | FM::RowMatrix tempX; 87 | FM::Vec i; 88 | FM::SymMatrix I; 89 | // allow fast operation if z_size remains constant 90 | FM::SymMatrix ZI; 91 | std::size_t last_z_size; 92 | void observe_size (std::size_t z_size); 93 | }; 94 | 95 | 96 | }//namespace 97 | #endif 98 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/infRtFlt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_INFORMATION_ROOT 2 | #define _BAYES_FILTER_INFORMATION_ROOT 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Information Root Filter Scheme. 14 | * A extended 'Square-root' Information filter as an Abstract class 15 | * 16 | * Algorithm: Square-root information propagation using QR factorisation 17 | * Ref: P. Dyer and S. McReynolds, "Extension of Square-Root Filtering to Include Process Noise", 18 | * [1] Journal of Optimization Theory and Applications, Vol.3 No.6 1969 19 | * Filter maintains r,R where 20 | * inv(R)*inv(R)' = X 21 | * r = R*x 22 | * R is upper triangular but not strictly a Cholesky factor as diagonal may be negative 23 | * Observe algorithm has been extended to include linearised models 24 | * Discontinuous observe models require that state is normalised with respect to the observation. 25 | * 26 | * The filter is operated by performing a 27 | * predict, observe 28 | * cycle defined by the base class 29 | */ 30 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 31 | 32 | /* Filter namespace */ 33 | namespace Bayesian_filter 34 | { 35 | 36 | class Information_root_scheme : public Extended_kalman_filter 37 | { 38 | public: 39 | FM::Vec r; // Information Root state 40 | FM::UTriMatrix R; // Information Root 41 | 42 | Information_root_scheme (std::size_t x_size, std::size_t z_initialsize = 0); 43 | 44 | void init (); 45 | void update (); 46 | // Covariance form state interface 47 | 48 | Float predict (Linrz_predict_model& f, const FM::ColMatrix& invFx, bool linear_r); 49 | /* Generalised form, using precomputed inverse of f.Fx */ 50 | Float predict (Linrz_predict_model& f); 51 | /* Use linrz form for r, computes inverse model using inverse_Fx */ 52 | Float predict (Linear_predict_model& f); 53 | /* Use linear form for r, computes inverse model using inverse_Fx */ 54 | Float predict (Linear_invertable_predict_model& f) 55 | /* Use linear form for r, and use inv.Fx from invertible model */ 56 | { 57 | return predict(f, f.inv.Fx, true); 58 | } 59 | 60 | Float observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s); 61 | Float observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s); 62 | // Extended_kalman_filter observe 63 | 64 | static void inverse_Fx (FM::DenseColMatrix& invFx, const FM::Matrix& Fx); 65 | /* Numerical Inversion of Fx using LU factorisation */ 66 | }; 67 | 68 | 69 | /* 70 | * Information Root Filter Scheme with exposed information state 71 | * Augments Information_root_filter with y,Y in the interface 72 | */ 73 | 74 | class Information_root_info_scheme : public Information_root_scheme, virtual public Information_state_filter 75 | { 76 | public: 77 | Information_root_info_scheme (std::size_t x_size, std::size_t z_initialsize = 0); 78 | 79 | void init_yY (); 80 | void update_yY (); 81 | // Information form state interface 82 | }; 83 | 84 | 85 | }//namespace 86 | #endif 87 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/itrFlt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_ITERATED_COVARIANCE 2 | #define _BAYES_FILTER_ITERATED_COVARIANCE 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Iterated Covariance Filter Scheme. 14 | * A non-linear Covariance (Kalman) filter with relinearisation and iteration 15 | * 16 | * The observe algorithm uses the iterated non-linear formulation 17 | * from Bar-Shalom and Fortmann p.119 (full scheme) 18 | * Discontinuous observe models require that state is normalised with 19 | * respect to the observation. 20 | * 21 | * The filter is operated by performing a 22 | * predict, observe 23 | * cycle defined by the base class 24 | */ 25 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 26 | 27 | /* Filter namespace */ 28 | namespace Bayesian_filter 29 | { 30 | 31 | class Iterated_covariance_scheme; 32 | 33 | class Iterated_observe_model : virtual public Observe_model_base 34 | /* Linrz observation model which can be iterated 35 | Hx can be relinearised 36 | */ 37 | { 38 | protected: // model is not sufficient, it is used to build observe model's 39 | Iterated_observe_model () 40 | {} 41 | public: 42 | virtual void relinearise (const FM::Vec& x) = 0; 43 | // Relinearised about state x 44 | }; 45 | 46 | 47 | class Iterated_terminator : public Bayes_base 48 | /* 49 | * Termination condition for filter Iteration 50 | * Used by iterated observe to parameterise termination condition 51 | * If iteration continues, the terminator must also relinearise the model about the filters new state 52 | * 53 | * Defaults to immediately terminating the iteration 54 | * 55 | * A more useful terminator can built by derivation. 56 | * For example terminator constructed with a reference to the filter and model can 57 | * detect convergence of x and/or X 58 | */ 59 | { 60 | public: 61 | virtual bool term_or_relinearize (const Iterated_covariance_scheme& f) 62 | { 63 | return true; 64 | } 65 | }; 66 | 67 | class Counted_iterated_terminator : public Iterated_terminator 68 | /* Termination condition with a simple fixed number of iterations 69 | */ 70 | { 71 | public: 72 | Counted_iterated_terminator (Iterated_observe_model& model, unsigned iterations) : 73 | m(model), i(iterations) 74 | {} 75 | bool term_or_relinearize (const Iterated_covariance_scheme& f); 76 | Iterated_observe_model& m; 77 | unsigned i; 78 | }; 79 | 80 | 81 | 82 | class Iterated_covariance_scheme : public Linrz_kalman_filter 83 | { 84 | public: 85 | Iterated_covariance_scheme (std::size_t x_size, std::size_t z_initialsize = 0); 86 | /* Initialised filter requires an addition iteration limit for the 87 | observe algorithm */ 88 | Iterated_covariance_scheme& operator= (const Iterated_covariance_scheme&); 89 | // Optimise copy assignment to only copy filter state 90 | 91 | void init (); 92 | void update (); 93 | Float predict (Linrz_predict_model& f); 94 | 95 | Float observe (Linrz_uncorrelated_observe_model& h, Iterated_terminator& term, const FM::Vec& z); 96 | Float observe (Linrz_correlated_observe_model& h, Iterated_terminator& term, const FM::Vec& z); 97 | // Observe with iteration 98 | Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z) 99 | { // Observe with default termination 100 | Iterated_terminator term; 101 | return observe (h, term, z); 102 | } 103 | Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z) 104 | { // Observe with default termination 105 | Iterated_terminator term; 106 | return observe (h, term, z); 107 | } 108 | 109 | public: // Exposed Numerical Results 110 | FM::SymMatrix S, SI; // Innovation Covariance and Inverse 111 | 112 | protected: // Permanently allocated temps 113 | FM::RowMatrix tempX; 114 | 115 | protected: // allow fast operation if z_size remains constant 116 | std::size_t last_z_size; 117 | void observe_size (std::size_t z_size); 118 | // Permanently allocated temps 119 | FM::Vec s; 120 | FM::Matrix HxT; 121 | }; 122 | 123 | 124 | }//namespace 125 | #endif 126 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/matSup.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_MATRIX_SUPPORT 2 | #define _BAYES_FILTER_MATRIX_SUPPORT 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Matrix support functions for filter classes 14 | * Members of the Bayesian_filter_matrix namespace are used in the 15 | * interface of Bayesian_filter and for internal operations. 16 | * Be aware! These functions and their implementation are more likely to change 17 | * then those in Bayesian_filter. 18 | */ 19 | 20 | /* Filter Matrix Namespace */ 21 | namespace Bayesian_filter_matrix 22 | { 23 | 24 | 25 | /* 26 | * Assertion support 27 | */ 28 | #ifndef NDEBUG 29 | void assert_isPSD (const SymMatrix &M); 30 | #else 31 | inline void assert_isPSD (const SymMatrix &M) {} 32 | #endif 33 | 34 | /* 35 | * Local support functions 36 | */ 37 | bool isPSD (const SymMatrix &M); 38 | bool isSymmetric (const Matrix &M); 39 | void forceSymmetric (Matrix &M, bool bUpperToLower = false); 40 | 41 | /* 42 | * UdU' and LdL' and UU' Cholesky Factorisation and function 43 | * Very important to manipulate PD and PSD matrices 44 | * 45 | * Return values: 46 | * Many algorithms return a value_type which is a reciprocal condition number 47 | * These values are documented for each algorithm and are important way to 48 | * determine the validity of the results 49 | */ 50 | Vec::value_type UdUrcond (const Vec& d); 51 | RowMatrix::value_type UdUrcond (const RowMatrix& UD); 52 | RowMatrix::value_type UdUrcond (const RowMatrix& UD, std::size_t n); 53 | UTriMatrix::value_type UCrcond (const UTriMatrix& UC); 54 | SymMatrix::value_type UdUdet (const SymMatrix& UD); 55 | 56 | // In-place factorisations 57 | RowMatrix::value_type UdUfactor_variant1 (RowMatrix& M, std::size_t n); 58 | RowMatrix::value_type UdUfactor_variant2 (RowMatrix& M, std::size_t n); 59 | inline RowMatrix::value_type UdUfactor (RowMatrix& M, std::size_t n) 60 | { return UdUfactor_variant2(M,n); 61 | } 62 | LTriMatrix::value_type LdLfactor (LTriMatrix& M, std::size_t n); 63 | UTriMatrix::value_type UCfactor (UTriMatrix& M, std::size_t n); 64 | 65 | // Copy factorisations 66 | RowMatrix::value_type UdUfactor (RowMatrix& UD, const SymMatrix& M); 67 | LTriMatrix::value_type LdLfactor (LTriMatrix& LD, const SymMatrix& M); 68 | UTriMatrix::value_type UCfactor (UTriMatrix& UC, const SymMatrix& M); 69 | 70 | // Factor manipulations 71 | bool UdUinverse (RowMatrix& UD); 72 | bool UTinverse (UTriMatrix& U); 73 | void UdUrecompose_transpose (RowMatrix& M); 74 | void UdUrecompose (RowMatrix& M); 75 | void UdUrecompose (SymMatrix& X, const RowMatrix& M); 76 | void UdUfromUCholesky (RowMatrix& U); 77 | void UdUseperate (RowMatrix& U, Vec& d, const RowMatrix& UD); 78 | void Lzero (RowMatrix& M); 79 | void Uzero (RowMatrix& M); 80 | 81 | /* 82 | * Functions using UdU factorisation: 83 | * inverse of Positive Definite matrix returning rcond 84 | */ 85 | SymMatrix::value_type UdUinversePDignoreInfinity (SymMatrix& M); 86 | SymMatrix::value_type UdUinversePD (SymMatrix& M); 87 | SymMatrix::value_type UdUinversePD (SymMatrix& M, SymMatrix::value_type& detM); 88 | SymMatrix::value_type UdUinversePD (SymMatrix& MI, const SymMatrix& M); 89 | SymMatrix::value_type UdUinversePD (SymMatrix& MI, SymMatrix::value_type& detM, const SymMatrix& M); 90 | 91 | 92 | }//namespace 93 | 94 | #endif 95 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/matSupSub.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $Id$ 7 | */ 8 | 9 | /* 10 | * Matrix types for filter classes 11 | * Provides the predefined type 'Vec' and a variety of 'Matrix' types 12 | * Replace this header to substitute alternative matrix support 13 | * 14 | * Everything in namespace Bayes_filter_matrix is intended to support the matrix storage 15 | * and algebra requirements of the library. Therefore the interfaces and implementation is 16 | * not intended to be stable. 17 | */ 18 | 19 | /* 20 | * Use the Boost uBLAS Basic Linear Algebra library 21 | * That is boost::numeric::ublas 22 | * Thanks to Joerg Walter and Mathias Koch for an excellent library! 23 | * 24 | * Gappy matrix support: The macros BAYES_FILTER_(SPARSE/COMPRESSED/COORDINATE) control experimental gappy matrix support 25 | * When enabled the default storage types are replaced with their sparse equivalents 26 | */ 27 | 28 | #include 29 | #if !(BOOST_VERSION >= 103200) 30 | #error Requires Boost 1.32.0 or later 31 | #endif 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #if defined(BAYES_FILTER_MAPPED) || defined(BAYES_FILTER_COMPRESSED) || defined(BAYES_FILTER_COORDINATE) 41 | #include 42 | #include 43 | #include 44 | #define BAYES_FILTER_GAPPY 45 | #endif 46 | 47 | 48 | 49 | /* Filter Matrix Namespace */ 50 | namespace Bayesian_filter_matrix 51 | { 52 | // Allow use of a local ublas namespace 53 | namespace ublas = boost::numeric::ublas; 54 | 55 | /* 56 | * Declare the value used for ALL linear algebra operations 57 | * Also required as the matrix/vector container value_type 58 | */ 59 | typedef double Float; 60 | 61 | /* 62 | * uBlas base types - these will be wrapper to provide the actual vector and matrix types 63 | * Symmetric types don't appear. They are defined later by adapting these base types 64 | */ 65 | namespace detail { 66 | // Dense types 67 | typedef ublas::vector BaseDenseVector; 68 | typedef ublas::matrix BaseDenseRowMatrix; 69 | typedef ublas::matrix BaseDenseColMatrix; 70 | typedef ublas::triangular_matrix BaseDenseUpperTriMatrix; 71 | typedef ublas::triangular_matrix BaseDenseLowerTriMatrix; 72 | typedef ublas::banded_matrix BaseDenseDiagMatrix; 73 | // Mapped types 74 | #if defined(BAYES_FILTER_MAPPED) 75 | typedef ublas::mapped_vector > BaseSparseVector; 76 | typedef ublas::mapped_matrix > BaseSparseRowMatrix; 77 | typedef ublas::mapped_matrix > BaseSparseColMatrix; 78 | // OR Compressed types 79 | #elif defined(BAYES_FILTER_COMPRESSED) 80 | typedef ublas::compressed_vector BaseSparseVector; 81 | typedef ublas::compressed_matrix BaseSparseRowMatrix; 82 | typedef ublas::compressed_matrix BaseSparseColMatrix; 83 | // OR Coordinate types 84 | #elif defined(BAYES_FILTER_COORDINATE) 85 | typedef ublas::coordinate_vector BaseSparseVector; 86 | typedef ublas::coordinate_matrix BaseSparseRowMatrix; 87 | typedef ublas::coordinate_matrix BaseSparseColMatrix; 88 | #endif 89 | 90 | // Default types Dense or Gappy 91 | #ifndef BAYES_FILTER_GAPPY 92 | typedef BaseDenseVector BaseVector; 93 | typedef BaseDenseRowMatrix BaseRowMatrix; 94 | typedef BaseDenseColMatrix BaseColMatrix; 95 | typedef BaseDenseUpperTriMatrix BaseUpperTriMatrix; 96 | typedef BaseDenseLowerTriMatrix BaseLowerTriMatrix; 97 | typedef BaseDenseDiagMatrix BaseDiagMatrix; 98 | #else 99 | typedef BaseSparseVector BaseVector; 100 | typedef BaseSparseRowMatrix BaseRowMatrix; 101 | typedef BaseSparseColMatrix BaseColMatrix; 102 | typedef BaseDenseUpperTriMatrix BaseUpperTriMatrix; // No sparse triangular or banded 103 | typedef BaseDenseLowerTriMatrix BaseLowerTriMatrix; 104 | typedef BaseDenseDiagMatrix BaseDiagMatrix; 105 | #endif 106 | 107 | } 108 | 109 | }//namespace 110 | 111 | /* 112 | * Common type independent uBlas interface 113 | */ 114 | #include "bayes_tracking/BayesFilter/uBLASmatrix.hpp" 115 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/random.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $Id$ 7 | */ 8 | 9 | /* 10 | * Good random numbers from Boost 11 | * Provides a common class for all random number requirements to test Bayes++ 12 | */ 13 | 14 | #ifndef Bayesian_filter_RANDOM_HPP 15 | #define Bayesian_filter_RANDOM_HPP 16 | 17 | #include 18 | #include 19 | 20 | 21 | namespace Bayesian_filter_test 22 | { 23 | 24 | namespace 25 | { 26 | template 27 | class simple_generator 28 | { 29 | public: 30 | typedef typename Distribution::result_type result_type; 31 | simple_generator(Engine& e, Distribution& d) 32 | : _eng(e), _dist(d) 33 | {} 34 | result_type operator()() 35 | { return _dist(_eng); 36 | } 37 | private: 38 | Engine& _eng; 39 | Distribution& _dist; 40 | }; 41 | }//namespace 42 | 43 | class Boost_random 44 | { 45 | public: 46 | typedef Bayesian_filter_matrix::Float Float; 47 | typedef boost::mt19937 URng; 48 | Boost_random() : dist_uniform_01(), dist_normal_01() 49 | {} 50 | Bayesian_filter_matrix::Float normal(const Float mean, const Float sigma) 51 | { 52 | boost::normal_distribution dist(mean, sigma); 53 | return dist(rng); 54 | } 55 | void normal(Bayesian_filter_matrix::DenseVec& v, const Float mean, const Float sigma) 56 | { 57 | boost::normal_distribution dist(mean, sigma); 58 | simple_generator > gen(rng, dist); 59 | std::generate (v.begin(), v.end(), gen); 60 | } 61 | void normal(Bayesian_filter_matrix::DenseVec& v) 62 | { 63 | simple_generator > gen(rng, dist_normal_01); 64 | std::generate (v.begin(), v.end(), gen); 65 | } 66 | void uniform_01(Bayesian_filter_matrix::DenseVec& v) 67 | { 68 | simple_generator > gen(rng, dist_uniform_01); 69 | std::generate (v.begin(), v.end(), gen); 70 | } 71 | #ifdef BAYES_FILTER_GAPPY 72 | void normal(Bayesian_filter_matrix::Vec& v, const Float mean, const Float sigma) 73 | { 74 | boost::normal_distribution dist(mean, sigma); 75 | simple_generator_01 > gen(rng, dist); 76 | for (std::size_t i = 0, iend=v.size(); i < iend; ++i) 77 | v[i] = gen(); 78 | } 79 | void normal(Bayesian_filter_matrix::Vec& v) 80 | { 81 | simple_generator_01 > gen(rng, dist_normal_01); 82 | for (std::size_t i = 0, iend=v.size(); i < iend; ++i) 83 | v[i] = gen(); 84 | } 85 | void uniform_01(Bayesian_filter_matrix::Vec& v) 86 | { 87 | simple_generator > gen(rng, dist_uniform_01); 88 | for (std::size_t i = 0, iend=v.size(); i < iend; ++i) 89 | v[i] = gen(); 90 | } 91 | #endif 92 | void seed() 93 | { 94 | rng.seed(); 95 | } 96 | private: 97 | URng rng; 98 | boost::uniform_01 dist_uniform_01; 99 | boost::normal_distribution dist_normal_01; 100 | }; 101 | 102 | 103 | }//namespace 104 | 105 | #endif 106 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/schemeFlt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_SCHEME 2 | #define _BAYES_FILTER_SCHEME 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Generic Filter 14 | * Filter schemes vary in their constructor parameterisation 15 | * Filter_scheme derives a generic filter with consistent constructor interface 16 | * 17 | * Provides specialisations for all Bayesian_filter schemes 18 | */ 19 | 20 | 21 | /* Filter namespace */ 22 | namespace Bayesian_filter 23 | { 24 | 25 | template 26 | struct Filter_scheme : public Scheme 27 | /* 28 | * A Generic Filter Scheme 29 | * Class template to provide a consistent constructor interface 30 | * Default for Kalman_state_filter 31 | */ 32 | { 33 | Filter_scheme(std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize) : 34 | Kalman_state_filter (x_size), Scheme (x_size, z_initialsize) 35 | {} 36 | }; 37 | 38 | 39 | // UD_filter specialisation 40 | template <> 41 | struct Filter_scheme : public UD_scheme 42 | { 43 | Filter_scheme(std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize) : 44 | Kalman_state_filter (x_size), UD_scheme (x_size, q_maxsize, z_initialsize) 45 | {} 46 | }; 47 | 48 | // Information_scheme specialisation 49 | template <> 50 | struct Filter_scheme : public Information_scheme 51 | { 52 | Filter_scheme(std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize) : 53 | Kalman_state_filter (x_size), Information_state_filter (x_size), 54 | Information_scheme (x_size, z_initialsize) 55 | {} 56 | }; 57 | 58 | // Information_root_info_scheme specialisation 59 | template <> 60 | struct Filter_scheme : public Information_root_info_scheme 61 | { 62 | Filter_scheme(std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize) : 63 | Kalman_state_filter (x_size), Information_state_filter (x_size), 64 | Information_root_info_scheme (x_size, z_initialsize) 65 | {} 66 | }; 67 | 68 | // SIR_scheme specialisation, inconsistent constructor 69 | template <> 70 | struct Filter_scheme : public SIR_scheme 71 | { 72 | Filter_scheme(std::size_t x_size, std::size_t s_size, SIR_random& random_helper) : 73 | Sample_state_filter (x_size, s_size), 74 | SIR_scheme (x_size, s_size, random_helper) 75 | {} 76 | }; 77 | 78 | // SIR_kalman_scheme specialisation, inconsistent constructor 79 | template <> 80 | struct Filter_scheme : public SIR_kalman_scheme 81 | { 82 | Filter_scheme(std::size_t x_size, std::size_t s_size, SIR_random& random_helper) : 83 | Sample_state_filter (x_size, s_size), 84 | Kalman_state_filter (x_size), 85 | SIR_kalman_scheme (x_size, s_size, random_helper) 86 | {} 87 | }; 88 | 89 | 90 | }//namespace 91 | #endif 92 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/uLAPACK.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $Id$ 7 | */ 8 | 9 | /* 10 | * uBLAS to LAPACK Interface 11 | * Very basic we only functions for information_root_filter supported 12 | */ 13 | 14 | /* Filter Matrix Namespace */ 15 | namespace Bayesian_filter_matrix 16 | { 17 | namespace LAPACK { 18 | 19 | namespace rawLAPACK { 20 | // LAPACK routines as C callable functions 21 | extern "C" { 22 | 23 | void dgeqrf_( 24 | const int & m, 25 | const int & n, 26 | double da[], 27 | const int & lda, 28 | double dtau[], 29 | double dwork[], 30 | const int& ldwork, 31 | int& info); 32 | void sgeqrf_( 33 | const int & m, 34 | const int & n, 35 | float da[], 36 | const int & lda, 37 | float dtau[], 38 | float dwork[], 39 | const int& ldwork, 40 | int& info); 41 | 42 | void dgetrs_( 43 | const char& transa, 44 | const int& n, 45 | const int& nrhs, 46 | const double da[], 47 | const int& lda, 48 | int ipivot[], 49 | double db[], 50 | const int& ldb, 51 | int& info); 52 | void sgetrs_( 53 | const char& transa, 54 | const int& n, 55 | const int& nrhs, 56 | const float da[], 57 | const int& lda, 58 | int ipivot[], 59 | float db[], 60 | const int& ldb, 61 | int& info); 62 | 63 | void dgetrf_( 64 | const int& m, 65 | const int& n, 66 | double da[], 67 | const int& lda, 68 | int ipivot[], 69 | int& info); 70 | void sgetrf_( 71 | const int& m, 72 | const int& n, 73 | float da[], 74 | const int& lda, 75 | int ipivot[], 76 | int& info); 77 | 78 | }// extern "C" 79 | 80 | // Type overloads for C++ 81 | inline void geqrf( const int & m, const int & n, double da[], const int & lda, double dtau[], double dwork[], const int& ldwork, int& info) 82 | { 83 | dgeqrf_(m,n,da,lda,dtau,dwork,ldwork,info); 84 | } 85 | inline void geqrf( const int & m, const int & n, float da[], const int & lda, float dtau[], float dwork[], const int& ldwork, int& info) 86 | { 87 | sgeqrf_(m,n,da,lda,dtau,dwork,ldwork,info); 88 | } 89 | inline void getrs( const char& transa, const int& n, const int& nrhs, const double da[], const int& lda, int ipivot[], double db[], const int& ldb, int& info) 90 | { 91 | dgetrs_(transa,n,nrhs,da,lda,ipivot,db,ldb,info); 92 | } 93 | inline void getrs( const char& transa, const int& n, const int& nrhs, const float da[], const int& lda, int ipivot[], float db[], const int& ldb, int& info) 94 | { 95 | sgetrs_(transa,n,nrhs,da,lda,ipivot,db,ldb,info); 96 | } 97 | inline void getrf( const int& m, const int& n, double da[], const int& lda, int ipivot[], int& info) 98 | { 99 | dgetrf_(m,n,da,lda,ipivot,info); 100 | } 101 | inline void getrf( const int& m, const int& n, float da[], const int& lda, int ipivot[], int& info) 102 | { 103 | sgetrf_(m,n,da,lda,ipivot,info); 104 | } 105 | }// namespace rawLAPACK 106 | 107 | 108 | /* Support types */ 109 | typedef boost::numeric::ublas::vector pivot_t; 110 | typedef Bayesian_filter_matrix::DenseVec vector_t; 111 | typedef Bayesian_filter_matrix::DenseColMatrix matrix_t; 112 | 113 | /* LAPACK Interface*/ 114 | 115 | 116 | 117 | // QR Factorization of a MxN General Matrix A. 118 | // a (IN/OUT - matrix(M,N)) On entry, the coefficient matrix A. On exit , the upper triangle and diagonal is the min(M,N) by N upper triangular matrix R. The lower triangle, together with the tau vector, is the orthogonal matrix Q as a product of min(M,N) elementary reflectors. 119 | // tau (OUT - vector (min(M,N))) Vector of the same numerical type as A. The scalar factors of the elementary reflectors. 120 | // info (OUT - int) 121 | // 0 : function completed normally 122 | // < 0 : The ith argument, where i = abs(return value) had an illegal value. 123 | int geqrf (matrix_t& a, vector_t& tau) 124 | { 125 | int _m = int(a.size1()); 126 | int _n = int(a.size2()); 127 | int _lda = int(a.size1()); 128 | int _info; 129 | 130 | // make_sure tau's size is greater than or equal to min(m,n) 131 | if (int(tau.size()) < (_n<_m ? _n : _m) ) 132 | return -104; 133 | 134 | int ldwork = _n*_n; 135 | vector_t dwork(ldwork); 136 | rawLAPACK::geqrf (_m, _n, a.data().begin(), _lda, tau.data().begin(), dwork.data().begin(), ldwork, _info); 137 | 138 | return _info; 139 | } 140 | 141 | // LU factorization of a general matrix A. 142 | // Computes an LU factorization of a general M-by-N matrix A using 143 | // partial pivoting with row interchanges. Factorization has the form 144 | // A = P*L*U. 145 | // a (IN/OUT - matrix(M,N)) On entry, the coefficient matrix A to be factored. On exit, the factors L and U from the factorization A = P*L*U. 146 | // ipivot (OUT - vector(min(M,N))) Integer vector. The row i of A was interchanged with row IPIV(i). 147 | // info (OUT - int) 148 | // 0 : successful exit 149 | // < 0 : If INFO = -i, then the i-th argument had an illegal value. 150 | // > 0 : If INFO = i, then U(i,i) is exactly zero. The factorization has been completed, but the factor U is exactly singular, and division by zero will occur if it is used to solve a system of equations. 151 | int getrf (matrix_t& a, pivot_t& ipivot) 152 | { 153 | matrix_t::value_type* _a = a.data().begin(); 154 | int _m = int(a.size1()); 155 | int _n = int(a.size2()); 156 | int _lda = _m; // minor size 157 | int _info; 158 | 159 | rawLAPACK::getrf (_m, _n, _a, _lda, ipivot.data().begin(), _info); 160 | 161 | return _info; 162 | } 163 | 164 | // Solution to a system using LU factorization 165 | // Solves a system of linear equations A*X = B with a general NxN 166 | // matrix A using the LU factorization computed by GETRF. 167 | // transa (IN - char) 'T' for the transpose of A, 'N' otherwise. 168 | // a (IN - matrix(M,N)) The factors L and U from the factorization A = P*L*U as computed by GETRF. 169 | // ipivot (IN - vector(min(M,N))) Integer vector. The pivot indices from GETRF; row i of A was interchanged with row IPIV(i). 170 | // b (IN/OUT - matrix(ldb,NRHS)) Matrix of same numerical type as A. On entry, the right hand side matrix B. On exit, the solution matrix X. 171 | // 172 | // info (OUT - int) 173 | // 0 : function completed normally 174 | // < 0 : The ith argument, where i = abs(return value) had an illegal value. 175 | // > 0 : if INFO = i, U(i,i) is exactly zero; the matrix is singular and its inverse could not be computed. 176 | int getrs (char transa, matrix_t& a, 177 | pivot_t& ipivot, matrix_t& b) 178 | { 179 | matrix_t::value_type* _a = a.data().begin(); 180 | int a_n = int(a.size1()); 181 | int _lda = a_n; 182 | int p_n = int(ipivot.size()); 183 | 184 | matrix_t::value_type* _b = b.data().begin(); 185 | int b_n = int(b.size1()); 186 | int _ldb = b_n; 187 | int _nrhs = int(b.size2()); /* B's size2 is the # of vectors on rhs */ 188 | 189 | if (a_n != b_n) /*Test to see if AX=B has correct dimensions */ 190 | return -101; 191 | if (p_n < a_n) /*Check to see if ipivot is big enough */ 192 | return -102; 193 | 194 | int _info; 195 | rawLAPACK::getrs (transa, a_n, _nrhs, _a, _lda, ipivot.data().begin(), 196 | _b, _ldb, _info); 197 | 198 | return _info; 199 | } 200 | 201 | }//namespace LAPACK 202 | }//namespace Bayesian_filter_matrix 203 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/BayesFilter/unsFlt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BAYES_FILTER_UNSCENTED 2 | #define _BAYES_FILTER_UNSCENTED 3 | 4 | /* 5 | * Bayes++ the Bayesian Filtering Library 6 | * Copyright (c) 2002 Michael Stevens 7 | * See accompanying Bayes++.htm for terms and conditions of use. 8 | * 9 | * $Id$ 10 | */ 11 | 12 | /* 13 | * Unscented Filter Scheme. 14 | * A Julier-Uhlmann Unscented non-linear Kalman filter 15 | * Uses the classic implementation of the Duplex Unscented transform. 16 | * The Unscented transform is used for non-linear state and observation predictions 17 | * 18 | * Observations are fused using innovation gain equations from a Covariance filter 19 | * 20 | * Predictions of state and state covariance (and observation) use 21 | * Unscented transformations to interpolate the non-linear predict and observe 22 | * models. Unscented transforms can be further optimised by vary the Kappa 23 | * parameter from its usual value of 1. 24 | * Discontinuous observe models require that a normalisation function. 25 | * 26 | * The predict model is represented by the state prediction function and a 27 | * separate prediction noise matrix. 28 | * The observe model is represented by the observation prediction function and 29 | * a function to normalise observations. 30 | * 31 | * The filter is operated by performing a 32 | * predict, observe 33 | * cycle defined by the base class 34 | */ 35 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 36 | 37 | /* Filter namespace */ 38 | namespace Bayesian_filter 39 | { 40 | 41 | class Unscented_predict_model : public Predict_model_base 42 | /* Specific Unscented prediction model for Additive noise 43 | * x(k|k-1) = f(x(k-1|k-1)) + w(x(k)) 44 | * 45 | * Unscented filter requires 46 | * f the function part of the non-linear model 47 | * Q the covariance of the additive w(x(k)), w is specifically allow to be a function of state 48 | */ 49 | { 50 | public: 51 | Unscented_predict_model (std::size_t q_size) 52 | { 53 | q_unscented = q_size; 54 | } 55 | 56 | virtual const FM::Vec& f(const FM::Vec& x) const = 0; 57 | // Functional part of additive model 58 | // Note: Reference return value as a speed optimisation, MUST be copied by caller. 59 | 60 | virtual const FM::SymMatrix& Q(const FM::Vec& x) const = 0; 61 | // Covariance of additive noise 62 | // Note: Reference return value as a speed optimisation, MUST be copied by caller. 63 | private: 64 | friend class Unscented_filter; // Filter implementation need to know noise size 65 | std::size_t q_unscented; 66 | }; 67 | 68 | 69 | class Unscented_scheme : public Linrz_kalman_filter, public Functional_filter 70 | { 71 | private: 72 | std::size_t q_max; // Maximum size allocated for noise model, constructed before XX 73 | public: 74 | FM::ColMatrix XX; // Unscented form of state, with associated Kappa 75 | Float kappa; 76 | 77 | Unscented_scheme (std::size_t x_size, std::size_t z_initialsize = 0); 78 | Unscented_scheme& operator= (const Unscented_scheme&); 79 | // Optimise copy assignment to only copy filter state 80 | 81 | void init (); 82 | void init_XX (); 83 | void update (); 84 | void update_XX (Float kappa); 85 | 86 | void predict (Unscented_predict_model& f); 87 | // Efficient Unscented prediction 88 | void predict (Functional_predict_model& f); 89 | void predict (Additive_predict_model& f); 90 | Float predict (Linrz_predict_model& f) 91 | { // Adapt to use the more general additive model 92 | predict(static_cast(f)); 93 | return 1.; // Always well condition for additive predict 94 | } 95 | 96 | Float observe (Uncorrelated_additive_observe_model& h, const FM::Vec& z); 97 | Float observe (Correlated_additive_observe_model& h, const FM::Vec& z); 98 | // Unscented filter implements general additive observe models 99 | 100 | Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z) 101 | { // Adapt to use the more general additive model 102 | return observe (static_cast(h),z); 103 | } 104 | Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z) 105 | { // Adapt to use the more general additive model 106 | return observe (static_cast(h),z); 107 | } 108 | 109 | public: // Exposed Numerical Results 110 | FM::Vec s; // Innovation 111 | FM::SymMatrix S, SI; // Innovation Covariance and Inverse 112 | 113 | protected: 114 | virtual Float predict_Kappa (std::size_t size) const; 115 | virtual Float observe_Kappa (std::size_t size) const; 116 | /* Unscented Kappa values 117 | default uses the rule which minimise mean squared error of 4th order term 118 | */ 119 | 120 | protected: // allow fast operation if z_size remains constant 121 | std::size_t last_z_size; 122 | void observe_size (std::size_t z_size); 123 | 124 | private: 125 | void unscented (FM::ColMatrix& XX, const FM::Vec& x, const FM::SymMatrix& X, Float scale); 126 | /* Determine Unscented points for a distribution */ 127 | std::size_t x_size; 128 | std::size_t XX_size; // 2*x_size+1 129 | 130 | protected: // Permanently allocated temps 131 | FM::ColMatrix fXX; 132 | }; 133 | 134 | 135 | }//namespace 136 | #endif 137 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/angle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * (c) Michael Stevens 2002, 2003 3 | * $Header: /home/nbellotto/cvsroot/projects/libtrack/src/angle.h,v 1.1.1.1 2007/09/12 10:27:00 belush Exp $ 4 | * $NoKeywords: $ 5 | */ 6 | 7 | /* 8 | * Normalised Angular Arithmetic 9 | * Manipulates angles using a normalised -PI <= angle < PI representation 10 | * The algorithm has proven to be the most numerically acurate. 11 | * For implementations where the cost of fmod is low it is also the most 12 | * efficient in all cases. 13 | * The class is templatised for a model of real for which std::fmod and arithmatic 14 | * and comparison operation are defined. 15 | */ 16 | 17 | namespace angleArith 18 | { 19 | 20 | template 21 | class angle 22 | { 23 | private: 24 | real a; // normalised angle 25 | 26 | // mymod - provide the most efficient system mod function 27 | inline static real myfmod(real x, real y) 28 | { 29 | return std::fmod(x,y); 30 | } 31 | 32 | public: 33 | // The mathematical constants 34 | static const real Pi, Pi2, Deg2Rad; 35 | 36 | angle (const real& unNorm) 37 | /* Conversion constructor from an unnormalised angle */ 38 | { 39 | // normalised in range 40 | a = myfmod(unNorm, Pi2); 41 | if (a >= Pi) { 42 | a -= Pi2; 43 | } 44 | else if (a < -Pi) { 45 | a += Pi2; 46 | } 47 | } 48 | 49 | angle (const angle& right) 50 | /* Copy construtor */ 51 | { 52 | a = right.a; 53 | } 54 | 55 | const angle& operator= (const angle& right) 56 | /* Assigment */ 57 | { 58 | a = right.a; 59 | return *this; 60 | } 61 | 62 | operator real () const 63 | /* Automatic conversion to real normalised angle */ 64 | { 65 | return a; 66 | } 67 | 68 | real from (real a2) const 69 | /* Return the angle for which it's difference from a2 is normalised */ 70 | { 71 | // normalised difference 72 | angle diff(this->a - a2); 73 | 74 | // rebase to a2 75 | return a2 + real(diff); 76 | } 77 | 78 | }; 79 | 80 | /* 81 | * Angular constants. Only to sufficient accuracy for an IEEE double 82 | */ 83 | template 84 | const real angle::Pi = real(3.1415926535898); 85 | template 86 | const real angle::Pi2 = real(3.1415926535898) * 2; 87 | template 88 | const real angle::Deg2Rad = real(3.1415926535898) / 180; 89 | 90 | 91 | /* 92 | * Template function constructor for simple syntax 93 | */ 94 | 95 | template 96 | angle normAngle(const real& r) 97 | { 98 | return angle(r); 99 | } 100 | 101 | template 102 | const real Pi () 103 | { 104 | return angle::Pi; 105 | } 106 | 107 | template 108 | const real Pi2 () 109 | { 110 | return angle::Pi2; 111 | } 112 | 113 | template 114 | const real Deg2Rad () 115 | { 116 | return angle::Deg2Rad; 117 | } 118 | 119 | }//namespace angleArith 120 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/associationmatrix.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Copyright (C) 2011 by Nicola Bellotto * 3 | * nbellotto@lincoln.ac.uk * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the GNU General Public License as published by * 7 | * the Free Software Foundation; either version 2 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the GNU General Public License * 16 | * along with this program; if not, write to the * 17 | * Free Software Foundation, Inc., * 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 19 | ***************************************************************************/ 20 | #ifndef ASSOCIATIONMATRIX_H 21 | #define ASSOCIATIONMATRIX_H 22 | 23 | #include 24 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 25 | 26 | 27 | using namespace std; 28 | using namespace Bayesian_filter; 29 | using namespace Bayesian_filter_matrix; 30 | 31 | typedef enum{CORRELATION, MAHALANOBIS, CORRELATION_LOG} measure_t; 32 | 33 | 34 | /** 35 | Association matrix 36 | 37 | @author Nicola Bellotto 38 | */ 39 | class AssociationMatrix : public std::vector< std::vector > 40 | { 41 | public: 42 | /** 43 | * Constructor 44 | */ 45 | AssociationMatrix(); 46 | 47 | /** 48 | * Constructor 49 | * @param row Number of rows 50 | * @param col Number of columns 51 | */ 52 | AssociationMatrix(size_t row, size_t col); 53 | 54 | /** 55 | * Destructor 56 | * @return 57 | */ 58 | ~AssociationMatrix(); 59 | 60 | /** 61 | * Set matrix size 62 | * @param row Number of rows 63 | * @param col Number of columns 64 | */ 65 | void setSize(size_t row, size_t col); 66 | 67 | /** 68 | * Print matrix elements on standard output 69 | */ 70 | void print(); 71 | 72 | /** 73 | * Get the number of rows 74 | * @return Number of rows 75 | */ 76 | inline size_t getRowSize() { return RowSize; } 77 | 78 | /** 79 | * Get the number of columns 80 | * @return Number of columns 81 | */ 82 | inline size_t getColSize() { return ColSize; } 83 | 84 | /** 85 | * Calculate the Mahalanobis distance: 86 | * d = sqrt((v1-v2)' * inv(R1+R2) * (v1-v2)) 87 | * @param v1 First vector 88 | * @param R1 Covariance matrix of v1 89 | * @param v2 Second vector 90 | * @param R2 Covariance matrix of v2 91 | * @return Mahalanobis distance 92 | */ 93 | static double mahalanobis(const FM::Vec& v1, 94 | const FM::SymMatrix& R1, 95 | const FM::Vec& v2, 96 | const FM::SymMatrix& R2); 97 | 98 | /** 99 | * Calculate the Mahalanobis distance: 100 | * d = sqrt(s' * inv(S) * s) 101 | * @param s Innovation vector 102 | * @param S Covariance matrix of s 103 | * @return Mahalanobis distance 104 | */ 105 | static double mahalanobis(const FM::Vec& s, 106 | const FM::SymMatrix& S); 107 | 108 | /** 109 | * Calculate the correlation of the innovation: 110 | * c = exp(-0.5 * d^2) / sqrt(2pi * |S|) 111 | * where @p d is the Mahalanobis distance 112 | * @param s Innovation vector 113 | * @param S Covariance matrix of s 114 | * @return Correlation 115 | */ 116 | static double correlation(const FM::Vec& s, 117 | const FM::SymMatrix& S); 118 | 119 | /** 120 | * Calculate distance 121 | * d = s' * Si * s + ln|S| 122 | * See: Bogler Philip. Radar Principles with Applications to Tracking Systems, John Wiley & Sons, 1989. 123 | * @param s Innovation vector 124 | * @param S Covariance matrix of s 125 | * @return Distance 126 | */ 127 | static double correlation_log(const FM::Vec& s, const FM::SymMatrix& S); 128 | 129 | /** 130 | * Return value of gate from Chi-square distribution 131 | * @param dof Degree of freedom (size of the vector to be gated) 132 | * @return Gate value (square root of relative value in table of Chi-square distribution for P = 0.01) 133 | */ 134 | static double gate(int dof); 135 | 136 | /** 137 | * Compute Nearest Neighbour (NN) assignment on the association matrix. 138 | * The result is a vector of (row,col) pairs stored in @p NN, where the first 139 | * element corresponds to the pair with the best similarity measure 140 | * @param measure Association measure: 'CORRELATION_LOG' (default), 'CORRELATION' or 'MAHALANOBIS' 141 | */ 142 | void computeNN(measure_t measure); 143 | 144 | public: 145 | typedef struct { size_t row; size_t col; double match; } match_t; 146 | /** 147 | * Vector of pairs (row,col) and relative similarity match value 148 | * computed with @p computeNN(...) 149 | */ 150 | std::vector< match_t > NN; 151 | 152 | /** 153 | * Vector of unmatched rows indexes 154 | */ 155 | std::vector< size_t > URow; 156 | 157 | /** 158 | * Vector of unmatched columns indexes 159 | */ 160 | std::vector< size_t > UCol; 161 | 162 | private: 163 | size_t RowSize; // matrix row size 164 | size_t ColSize; // matrix column size 165 | static Float detS; 166 | }; 167 | 168 | // short name version 169 | typedef AssociationMatrix AM; 170 | 171 | 172 | #endif 173 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/ekfilter.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Copyright (C) 2006 by Nicola Bellotto * 3 | * nbellotto@lincoln.ac.uk * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the GNU General Public License as published by * 7 | * the Free Software Foundation; either version 2 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the GNU General Public License * 16 | * along with this program; if not, write to the * 17 | * Free Software Foundation, Inc., * 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 19 | ***************************************************************************/ 20 | #ifndef EKFILTER_H 21 | #define EKFILTER_H 22 | 23 | #include 24 | #include "bayes_tracking/BayesFilter/covFlt.hpp" 25 | 26 | 27 | using namespace Bayesian_filter; 28 | using namespace Bayesian_filter_matrix; 29 | 30 | /** 31 | EKF state estimator 32 | 33 | @author Nicola Bellotto 34 | */ 35 | class EKFilter : public Covariance_scheme 36 | { 37 | public: 38 | /** 39 | * Constructor - include initialization with null state and null covariance 40 | */ 41 | EKFilter(std::size_t x_size); 42 | 43 | /** 44 | * Constructor 45 | * @param x0 State vector 46 | * @param P0 Covariance matrix 47 | */ 48 | EKFilter(const FM::Vec& x0, const FM::SymMatrix& P0); 49 | 50 | /** 51 | * Destructor 52 | */ 53 | ~EKFilter(); 54 | 55 | /** 56 | * Initialize state and covariance 57 | * @param x0 State vector 58 | * @param P0 Covariance matrix 59 | */ 60 | void init(const FM::Vec& x0, const FM::SymMatrix& P0); 61 | 62 | /** 63 | * Prediction (include Jacobian update) 64 | * @param f Linearized model 65 | */ 66 | Bayes_base::Float predict (Linrz_predict_model& f); 67 | 68 | /** 69 | * Prediction (include Jacobian update) 70 | * @param f Gaussian model 71 | */ 72 | Bayes_base::Float predict (Gaussian_predict_model& f); 73 | 74 | /** 75 | * Perform prediction and correction to update the state 76 | * @param predict_model 77 | * @param observe_model 78 | * @param z 79 | */ 80 | void update(Linrz_predict_model& predict_model, 81 | Linrz_correlated_observe_model& observe_model, 82 | const FM::Vec& z); 83 | 84 | /** 85 | * Predict the observation from the last predicted state and the covariance R_p of the predicted observation 86 | * @param observe_model Observation model 87 | * @param z_pred Predicted observation (return) 88 | * @param R_pred Predicted observation covariance (return) 89 | */ 90 | void predict_observation(Linrz_correlated_observe_model& observe_model, 91 | FM::Vec& z_pred, FM::SymMatrix& R_pred); 92 | 93 | /** 94 | * Update the state from innovation and relative covariance. 95 | * @param h Observation model 96 | * @param si Innovation 97 | * @param Si Modified innovation covariance 98 | */ 99 | Bayes_base::Float observeInnovation(Linrz_correlated_observe_model& h, const Bayesian_filter_matrix::Vec& si, const Bayesian_filter_matrix::SymMatrix& Si); 100 | 101 | /** Override method */ 102 | Bayes_base::Float observe (Linrz_correlated_observe_model& h, const FM::Vec& z); 103 | /** Override method */ 104 | Bayes_base::Float observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z); 105 | 106 | /** 107 | * Return the logarithm of the (normalized) likelihhod after the last observation 108 | * @return Logarithm of the likelihood 109 | */ 110 | double logLikelihood(); 111 | 112 | public: 113 | /** Innovation */ 114 | FM::Vec s; 115 | /** Predicted observation */ 116 | FM::Vec z_p; 117 | 118 | private: 119 | std::size_t x_size; 120 | std::size_t X_size; 121 | }; 122 | 123 | #endif 124 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/jacobianmodel.h: -------------------------------------------------------------------------------- 1 | // 2 | // C++ Interface: jacobianmodel 3 | // 4 | // Description: 5 | // 6 | // 7 | // Author: Nicola Bellotto , (C) 2011 8 | // 9 | // Copyright: See COPYING file that comes with this distribution 10 | // 11 | // 12 | 13 | #ifndef JACOBIANMODEL_H 14 | #define JACOBIANMODEL_H 15 | 16 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 17 | 18 | using namespace Bayesian_filter_matrix; 19 | 20 | class JacobianModel { 21 | public: 22 | /** Contructor */ 23 | virtual ~JacobianModel(){}; 24 | 25 | /** 26 | * Update Jacobian matrix of the model 27 | * @p x state argument 28 | */ 29 | virtual void updateJacobian(const Vec& x) = 0; 30 | }; 31 | 32 | 33 | #endif // JACOBIANMODEL_H 34 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/pfilter.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Copyright (C) 2006 by Nicola Bellotto * 3 | * nbellotto@lincoln.ac.uk * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the GNU General Public License as published by * 7 | * the Free Software Foundation; either version 2 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the GNU General Public License * 16 | * along with this program; if not, write to the * 17 | * Free Software Foundation, Inc., * 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 19 | ***************************************************************************/ 20 | #ifndef PFILTER_H 21 | #define PFILTER_H 22 | 23 | #include "bayes_tracking/BayesFilter/SIRFlt.hpp" 24 | #include "bayes_tracking/BayesFilter/random.hpp" 25 | 26 | 27 | using namespace Bayesian_filter; 28 | using namespace Bayesian_filter_matrix; 29 | 30 | namespace PF 31 | { 32 | 33 | class Boost_random : public SIR_random, public Bayesian_filter_test::Boost_random 34 | /* 35 | * Random numbers for SIR from Boost 36 | */ 37 | { 38 | public: 39 | using Bayesian_filter_test::Boost_random::normal; 40 | void normal (DenseVec& v) 41 | { 42 | Bayesian_filter_test::Boost_random::normal (v); 43 | } 44 | using Bayesian_filter_test::Boost_random::uniform_01; 45 | void uniform_01 (DenseVec& v) 46 | { 47 | Bayesian_filter_test::Boost_random::uniform_01 (v); 48 | } 49 | }; 50 | 51 | } // namespace 52 | 53 | 54 | /** 55 | @author Nicola Bellotto 56 | */ 57 | class PFilter : public SIR_kalman_scheme 58 | { 59 | public: 60 | /** 61 | * Constructor - include initialization with null state and null covariance 62 | * @param x_size State size 63 | * @param s_size Number of samples 64 | * @param random_helper Random generator 65 | * @return 66 | */ 67 | PFilter(std::size_t x_size, 68 | std::size_t s_size, 69 | SIR_random &random_helper); 70 | 71 | /** 72 | * Constructor 73 | * @param x_size State size 74 | * @param s_size Number of samples (default 1000) 75 | */ 76 | PFilter(std::size_t x_size, 77 | std::size_t s_size = 1000); 78 | 79 | /** 80 | * Constructor 81 | * @param x0 State vector 82 | * @param P0 Covariance matrix 83 | * @param s_size Number of samples 84 | * @param random_helper Random generator 85 | */ 86 | PFilter(const FM::Vec& x0, 87 | const FM::SymMatrix& P0, 88 | std::size_t s_size, 89 | SIR_random& random_helper); 90 | 91 | /** 92 | * Constructor 93 | * @param x0 State vector 94 | * @param P0 Covariance matrix 95 | * @param s_size Number of samples (default 1000) 96 | */ 97 | PFilter(const FM::Vec& x0, 98 | const FM::SymMatrix& P0, 99 | std::size_t s_size = 1000); 100 | 101 | /** 102 | * Destructor 103 | */ 104 | ~PFilter(); 105 | 106 | PFilter& operator= (const PFilter& a) 107 | { 108 | SIR_scheme::operator=(a); 109 | x = a.x; 110 | X = a.X; 111 | return *this; 112 | } 113 | 114 | /** 115 | * Initialize state and covariance 116 | * @param x0 State vector 117 | * @param P0 Covariance matrix 118 | */ 119 | void init(const FM::Vec& x0, const FM::SymMatrix& P0); 120 | 121 | /** 122 | * Prediction the state 123 | * @param predict_model Prediction model 124 | */ 125 | void predict(Sampled_predict_model& predict_model); 126 | 127 | /** 128 | * Predict the observation from the last predicted state and the covariance R_p of the predicted observation 129 | * @param observe_model Observation model 130 | * @param z_pred Predicted observation (return) 131 | * @param R_pred Predicted observation covariance (return) 132 | */ 133 | void predict_observation(Correlated_additive_observe_model& observe_model, 134 | FM::Vec& z_pred, FM::SymMatrix& R_pred); 135 | 136 | /** 137 | * Perform correction of the predicted state according to the observation. 138 | * @param observe_model Observation model 139 | * @param z Current observation 140 | */ 141 | void observe(Likelihood_observe_model& observe_model, 142 | const FM::Vec& z); 143 | 144 | // void roughen() 145 | // // Generalised roughening: Default to roughen_minmax 146 | // { 147 | // if (rougheningK != 0) 148 | // roughen_minmax (S, rougheningK); 149 | // } 150 | 151 | /** 152 | * Return the logarithm of the likelihhod for the last observation 153 | * @return Logarithm of the likelihood 154 | */ 155 | double logLikelihood(); 156 | 157 | public: 158 | /** Particle weights */ 159 | FM::DenseVec& w; 160 | 161 | private: 162 | std::size_t x_size; 163 | PF::Boost_random rnd; 164 | double m_likelihood; 165 | }; 166 | 167 | #endif 168 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/trackwin.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Copyright (C) 2005 by Nicola Bellotto * 3 | * nbellotto@lincoln.ac.uk * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the GNU General Public License as published by * 7 | * the Free Software Foundation; either version 2 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the GNU General Public License * 16 | * along with this program; if not, write to the * 17 | * Free Software Foundation, Inc., * 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 19 | ***************************************************************************/ 20 | #ifndef TRACKWIN_H 21 | #define TRACKWIN_H 22 | 23 | 24 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 25 | #include "bayes_tracking/BayesFilter/matSup.hpp" 26 | #include 27 | #include 28 | #include 29 | 30 | #define WINDOW_WIDTH 640 31 | #define WINDOW_HEIGHT 480 32 | #define SCALE_FACTOR 30.0 33 | #define TEXT_LENGTH 128 34 | 35 | /** 36 | @author Nicola Bellotto 37 | */ 38 | 39 | using namespace std; 40 | using namespace Bayesian_filter_matrix; 41 | using namespace cv; 42 | 43 | 44 | class TrackWin { 45 | public: 46 | typedef enum {BLACK, WHITE, GREY, BLUE, GREEN, DARK_GREEN, RED, PURPLE, YELLOW, ORANGE, DARK_ORANGE} color_t; 47 | 48 | typedef struct 49 | { 50 | std::string label; // label 51 | double x; // x pos 52 | double y; // y pos 53 | double th; // orientation 54 | color_t color; // color 55 | double varx; // x variance 56 | double vary; // y variance 57 | double covxy; // xy covariance 58 | const ColMatrix* s; // samples 59 | } t_object; 60 | 61 | /** 62 | * Constructor 63 | * @param name Name of the window 64 | * @param width width of the window 65 | * @param height height of the window 66 | * @param scale scale of the window 67 | */ 68 | TrackWin(const char* name, 69 | int width = WINDOW_WIDTH, 70 | int height = WINDOW_HEIGHT, 71 | double scale = SCALE_FACTOR); 72 | 73 | /** 74 | * Destructor 75 | */ 76 | ~TrackWin(); 77 | 78 | /** 79 | * Create and visualize the window 80 | */ 81 | void create(); 82 | 83 | /** 84 | * Destroy the window 85 | */ 86 | void destroy(); 87 | 88 | /** 89 | * Set an object to be drawn 90 | * @param label Label 91 | * @param xpos X position 92 | * @param ypos Y position 93 | * @param orientation Orientation 94 | * @param color Color 95 | * @param varx X variance 96 | * @param vary Y variance 97 | * @param covxy XY covariance 98 | * @param samples sample from particle filter 99 | */ 100 | void setObject(const char* label, double xpos, double ypos, double orientation = NO_ORIENTATION, 101 | color_t color = BLACK, double varx = 0, double vary = 0, double covxy = 0, const ColMatrix* samples = NULL); 102 | 103 | /** 104 | * Set the origin (i.e. the observation point) 105 | * @param x0 X translation 106 | * @param y0 Y translation 107 | * @param th0 Rotation 108 | */ 109 | void setOrigin(double x0, double y0, double th0); 110 | 111 | /** 112 | * Write some text on the window 113 | * @param text Text string 114 | * @param color Text color 115 | */ 116 | void setText(const char* text, color_t color = BLACK); 117 | 118 | /** 119 | * Update the window's content 120 | * @param delay Time delay in [ms] for OpenCV' signal handling (default 5ms) 121 | * @param cameraView Angle of view of the camera 122 | * @param cx CX 123 | * @param cy CY 124 | */ 125 | void update(int delay = 5, double cameraView = -1, double cx = 0, double cy = 0); 126 | 127 | /** 128 | * Save a snapshot of the current window 129 | * @param filename Name of the image file 130 | */ 131 | void saveSnapshot(const char* filename); 132 | 133 | public: 134 | static const double NO_ORIENTATION; 135 | 136 | private: 137 | int m_winWidth; 138 | int m_winHeight; 139 | double m_scale; 140 | int m_xoffset; 141 | int m_yoffset; 142 | char* _name; 143 | bool _created; 144 | IplImage* _canvas; 145 | std::vector _objVector; 146 | double _x0; 147 | double _y0; 148 | double _th0; 149 | double _cosTh0; 150 | double _sinTh0; 151 | char* _text; 152 | color_t _textColor; 153 | CvPoint _textPos; 154 | CvFont _font; 155 | 156 | private: 157 | CvScalar getColor(color_t color); 158 | RotatedRect getErrorEllipse(double chisquare_val, Point2f mean, Mat covmat); 159 | }; 160 | 161 | #endif 162 | -------------------------------------------------------------------------------- /bayestracking/include/bayes_tracking/ukfilter.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Copyright (C) 2006 by Nicola Bellotto * 3 | * nbellotto@lincoln.ac.uk * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the GNU General Public License as published by * 7 | * the Free Software Foundation; either version 2 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the GNU General Public License * 16 | * along with this program; if not, write to the * 17 | * Free Software Foundation, Inc., * 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 19 | ***************************************************************************/ 20 | #ifndef UKFILTER_H 21 | #define UKFILTER_H 22 | 23 | #include 24 | #include "bayes_tracking/BayesFilter/unsFlt.hpp" 25 | 26 | 27 | using namespace Bayesian_filter; 28 | using namespace Bayesian_filter_matrix; 29 | 30 | /** 31 | UKF state estimator 32 | 33 | @author Nicola Bellotto 34 | */ 35 | class UKFilter : public Unscented_scheme 36 | { 37 | public: 38 | /** 39 | * Constructor - include initialization with null state and null covariance 40 | */ 41 | UKFilter(std::size_t x_size); 42 | 43 | /** 44 | * Constructor 45 | * @param x0 State vector 46 | * @param P0 Covariance matrix 47 | */ 48 | UKFilter(const FM::Vec& x0, const FM::SymMatrix& P0); 49 | 50 | /** 51 | * Destructor 52 | */ 53 | ~UKFilter(); 54 | 55 | /** 56 | * Initialize state and covariance 57 | * @param x0 State vector 58 | * @param P0 Covariance matrix 59 | */ 60 | void init(const FM::Vec& x0, const FM::SymMatrix& P0); 61 | 62 | /** 63 | * Perform prediction and correction to update the state 64 | * @param predict_model 65 | * @param observe_model 66 | * @param z 67 | */ 68 | void update(Additive_predict_model& predict_model, 69 | Correlated_additive_observe_model& observe_model, 70 | const FM::Vec& z); 71 | 72 | /** 73 | * Predict the observation from the last predicted state and the covariance R_p of the predicted observation 74 | * @param observe_model Observation model 75 | * @param z_pred Predicted observation (return) 76 | * @param R_pred Predicted observation covariance (return) 77 | */ 78 | void predict_observation(Correlated_additive_observe_model& observe_model, 79 | FM::Vec& z_pred, FM::SymMatrix& R_pred); 80 | 81 | /** 82 | * Update the state from innovation and relative covariance. 83 | * @param h Observation model 84 | * @param si Innovation 85 | * @param Si Modified innovation covariance 86 | */ 87 | Bayes_base::Float observeInnovation(Correlated_additive_observe_model& h, 88 | const Vec& si, const SymMatrix& Si); 89 | 90 | /** 91 | * Return the logarithm of the (normalized) likelihhod after the last observation 92 | * @return Logarithm of the likelihood 93 | */ 94 | double logLikelihood(); 95 | 96 | public: 97 | /** Predicted observation */ 98 | FM::Vec z_p; 99 | 100 | private: 101 | void unscented(FM::ColMatrix& XX, 102 | const FM::Vec& x, 103 | const FM::SymMatrix& X, 104 | Float scale); 105 | 106 | private: 107 | std::size_t x_size; 108 | std::size_t XX_size; 109 | }; 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /bayestracking/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bayes_tracking 4 | 1.0.5 5 | The bayestracking package uses extended or unscented Kalman filters to track movement. The constant velocity model has been designed to track the movement of people in cartesian space in particular. 6 | 7 | Christian Dondrup 8 | Marc Hanheide 9 | 10 | MIT 11 | 12 | https://github.com/LCAS/bayestracking 13 | http://dx.doi.org/10.5281/zenodo.10318 14 | 15 | Nicola Bellotto 16 | 17 | catkin 18 | boost 19 | libblas-dev 20 | cv_bridge 21 | boost 22 | libblas-dev 23 | cv_bridge 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/BayesFilter/CIFlt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $Id$ 7 | */ 8 | 9 | /* 10 | * Covariance Intersection Filter. 11 | * TODO: Implement useful Omega based on iterative optimization algorithm from the authors of reference [1] 12 | */ 13 | #include "bayes_tracking/BayesFilter/CIFlt.hpp" 14 | #include "bayes_tracking/BayesFilter/matSup.hpp" 15 | #include "bayes_tracking/BayesFilter/models.hpp" 16 | 17 | /* Filter namespace */ 18 | namespace Bayesian_filter 19 | { 20 | using namespace Bayesian_filter_matrix; 21 | 22 | 23 | CI_scheme::CI_scheme (std::size_t x_size, std::size_t z_initialsize) : 24 | Kalman_state_filter(x_size), 25 | S(Empty), SI(Empty) 26 | /* Initialise filter and set the size of things we know about 27 | */ 28 | { 29 | last_z_size = 0; // Matrices conform to z_initialsize, they are left Empty if z_initialsize==0 30 | observe_size (z_initialsize); 31 | } 32 | 33 | CI_scheme& CI_scheme::operator= (const CI_scheme& a) 34 | /* Optimise copy assignment to only copy filter state 35 | * Precond: matrix size conformance 36 | */ 37 | { 38 | Kalman_state_filter::operator=(a); 39 | return *this; 40 | } 41 | 42 | 43 | void CI_scheme::init () 44 | { 45 | // Postconditions 46 | if (!isPSD (X)) 47 | error (Numeric_exception("Initial X not PSD")); 48 | } 49 | 50 | void CI_scheme::update () 51 | { 52 | // Nothing to do, implicit in observation 53 | } 54 | 55 | Bayes_base::Float 56 | CI_scheme::predict (Linrz_predict_model& f) 57 | { 58 | x = f.f(x); // Extended Kalman state predict is f(x) directly 59 | // Predict state covariance 60 | RowMatrix tempX(f.Fx.size1(), X.size2()); 61 | noalias(X) = prod_SPD(f.Fx,X, tempX); 62 | noalias(X) += prod_SPD(f.G, f.q, tempX); 63 | 64 | return 1; 65 | } 66 | 67 | void CI_scheme::observe_size (std::size_t z_size) 68 | /* Optimised dynamic observation sizing 69 | */ 70 | { 71 | if (z_size != last_z_size) { 72 | last_z_size = z_size; 73 | 74 | S.resize(z_size,z_size, false); 75 | SI.resize(z_size,z_size, false); 76 | } 77 | } 78 | 79 | 80 | Bayes_base::Float 81 | CI_scheme::observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s) 82 | /* Iterated Extended Kalman Filter 83 | * Bar-Shalom and Fortmann p.119 (full scheme) 84 | * A hard limit is placed on the iterations whatever the 85 | * the normal terminal condition is to guarantee termination 86 | * Uncorrelated noise 87 | */ 88 | { 89 | // ISSUE: Implement simplified uncorrelated noise equations 90 | std::size_t z_size = s.size(); 91 | SymMatrix Z(z_size,z_size); 92 | 93 | Adapted_Linrz_correlated_observe_model hh(h); 94 | return observe_innovation (hh, s); 95 | } 96 | 97 | 98 | Bayes_base::Float 99 | CI_scheme::observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s) 100 | /* Correlated innovation observe 101 | */ 102 | { 103 | const Float one = 1; 104 | // size consistency, z to model 105 | if (s.size() != h.Z.size1()) 106 | error (Logic_exception("observation and model size inconsistent")); 107 | observe_size (s.size()); // dynamic sizing 108 | 109 | // Linear conditioning for omega 110 | SymMatrix invZ(h.Z.size1(),h.Z.size2()); 111 | Float rcond = UdUinversePD (invZ, h.Z); 112 | rclimit.check_PSD(rcond, "Z not PSD in observe"); 113 | 114 | Matrix HTinvZ (prod(trans(h.Hx), invZ)); 115 | SymMatrix HTinvZH (prod(HTinvZ, h.Hx)); 116 | 117 | SymMatrix invX(X.size1(),X.size2()); 118 | rcond = UdUinversePD (invX, X); 119 | rclimit.check_PD(rcond, "X not PD in observe"); 120 | 121 | 122 | // find omega 123 | Float omega = Omega(invX, HTinvZH, X); 124 | 125 | // calculate predicted innovation 126 | Matrix XHT (prod(X, trans(h.Hx))); 127 | Matrix HXHT (prod(h.Hx, XHT)); 128 | S = HXHT * (one-omega) + h.Z * omega; 129 | 130 | // inverse innovation covariance 131 | rcond = UdUinversePD (SI, S); 132 | rclimit.check_PD(rcond, "S not PD in observe"); 133 | 134 | Matrix K (prod(XHT*(one-omega), SI)); 135 | 136 | // state update 137 | noalias(x) += prod(K, s); 138 | // inverse covariance 139 | invX *= omega; 140 | noalias(invX) += HTinvZH*(one-omega); 141 | // covariance 142 | rcond = UdUinversePD (X, invX); 143 | rclimit.check_PD(rcond, "inverse covariance not PD in observe"); 144 | return rcond; 145 | } 146 | 147 | 148 | }//namespace 149 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/BayesFilter/bayesFlt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $Id$ 7 | */ 8 | 9 | /* 10 | * Implement bayesFlt.hpp : 11 | * constructor/destructors 12 | * error handlers 13 | * default virtual and member functions 14 | */ 15 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 16 | #include 17 | #include // Only for unique_samples 18 | 19 | 20 | /* Filter namespace */ 21 | namespace Bayesian_filter 22 | { 23 | 24 | 25 | /* Minimum allowable reciprocal condition number for PD Matrix factorisations 26 | * Initialised default gives 5 decimal digits of headroom */ 27 | const Bayes_base::Float Numerical_rcond::limit_PD_init = std::numeric_limits::epsilon() * Bayes_base::Float(1e5); 28 | 29 | 30 | Bayes_base::~Bayes_base() 31 | /* Default definition required for a pure virtual destructor 32 | */ 33 | {} 34 | 35 | void Bayes_base::error (const Numeric_exception& e ) 36 | /* Filter error 37 | */ 38 | { 39 | throw e; 40 | } 41 | 42 | void Bayes_base::error (const Logic_exception& e ) 43 | /* Filter error 44 | */ 45 | { 46 | throw e; 47 | } 48 | 49 | Gaussian_predict_model::Gaussian_predict_model (std::size_t x_size, std::size_t q_size) : 50 | q(q_size), G(x_size, q_size) 51 | /* Set the size of things we know about 52 | */ 53 | {} 54 | 55 | Additive_predict_model::Additive_predict_model (std::size_t x_size, std::size_t q_size) : 56 | q(q_size), G(x_size, q_size) 57 | /* 58 | * Set the size of things we know about 59 | */ 60 | {} 61 | 62 | Linrz_predict_model::Linrz_predict_model (std::size_t x_size, std::size_t q_size) : 63 | /* Set the size of things we know about 64 | */ 65 | Additive_predict_model(x_size, q_size), 66 | Fx(x_size,x_size) 67 | {} 68 | 69 | Linear_predict_model::Linear_predict_model (std::size_t x_size, std::size_t q_size) : 70 | /* Set the size of things we know about 71 | */ 72 | Linrz_predict_model(x_size, q_size), 73 | xp(x_size) 74 | {} 75 | 76 | Linear_invertable_predict_model::Linear_invertable_predict_model (std::size_t x_size, std::size_t q_size) : 77 | /* 78 | * Set the size of things we know about 79 | */ 80 | Linear_predict_model(x_size, q_size), 81 | inv(x_size) 82 | {} 83 | 84 | Linear_invertable_predict_model::inverse_model::inverse_model (std::size_t x_size) : 85 | Fx(x_size,x_size) 86 | {} 87 | 88 | 89 | State_filter::State_filter (std::size_t x_size) : 90 | x(x_size) 91 | /* Initialise filter and set the size of things we know about 92 | */ 93 | { 94 | if (x_size < 1) 95 | error (Logic_exception("Zero state filter constructed")); 96 | } 97 | 98 | 99 | Kalman_state_filter::Kalman_state_filter (std::size_t x_size) : 100 | /* Initialise state size 101 | */ 102 | State_filter(x_size), X(x_size,x_size) 103 | {} 104 | 105 | void Kalman_state_filter::init_kalman (const FM::Vec& x, const FM::SymMatrix& X) 106 | /* Initialise from a state and state covariance 107 | * Parameters that reference the instance's x and X members are valid 108 | */ 109 | { 110 | Kalman_state_filter::x = x; 111 | Kalman_state_filter::X = X; 112 | init (); 113 | } 114 | 115 | 116 | Bayes_base::Float 117 | Extended_kalman_filter::observe (Linrz_correlated_observe_model& h, const FM::Vec& z) 118 | /* Extended linrz correlated observe, compute innovation for observe_innovation 119 | */ 120 | { 121 | update (); 122 | const FM::Vec& zp = h.h(x); // Observation model, zp is predicted observation 123 | 124 | FM::Vec s = z; 125 | h.normalise(s, zp); 126 | FM::noalias(s) -= zp; 127 | return observe_innovation (h, s); 128 | } 129 | 130 | Bayes_base::Float 131 | Extended_kalman_filter::observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z) 132 | /* Extended Kalman uncorrelated observe, compute innovation for observe_innovation 133 | */ 134 | { 135 | update (); 136 | const FM::Vec& zp = h.h(x); // Observation model, zp is predicted observation 137 | 138 | FM::Vec s = z; 139 | h.normalise(s, zp); 140 | FM::noalias(s) -= zp; 141 | return observe_innovation (h, s); 142 | } 143 | 144 | 145 | Information_state_filter::Information_state_filter (std::size_t x_size) : 146 | /* Initialise state size 147 | */ 148 | y(x_size), Y(x_size,x_size) 149 | {} 150 | 151 | void Information_state_filter::init_information (const FM::Vec& y, const FM::SymMatrix& Y) 152 | /* Initialise from a information state and information 153 | * Parameters that reference the instance's y and Y members are valid 154 | */ 155 | { 156 | Information_state_filter::y = y; 157 | Information_state_filter::Y = Y; 158 | init_yY (); 159 | } 160 | 161 | 162 | Sample_state_filter::Sample_state_filter (std::size_t x_size, std::size_t s_size) : 163 | S(x_size,s_size) 164 | /* Initialise state size 165 | * Postcond: s_size >= 1 166 | */ 167 | { 168 | if (s_size < 1) 169 | error (Logic_exception("Zero sample filter constructed")); 170 | } 171 | 172 | Sample_state_filter::~Sample_state_filter() 173 | /* Default definition required for a pure virtual destructor 174 | * ISSUE Cannot be defined if Matrix has private destructor 175 | */ 176 | { 177 | } 178 | 179 | void Sample_state_filter::init_sample (const FM::ColMatrix& initS) 180 | /* Initialise from a sampling 181 | */ 182 | { 183 | S = initS; 184 | init_S(); 185 | } 186 | 187 | namespace { 188 | // Column proxy so S can be sorted indirectly 189 | struct ColProxy 190 | { 191 | const FM::ColMatrix* cm; 192 | std::size_t col; 193 | const ColProxy& operator=(const ColProxy& a) 194 | { 195 | col = a.col; 196 | return a; 197 | } 198 | // Provide a ordering on columns 199 | static bool less(const ColProxy& a, const ColProxy& b) 200 | { 201 | FM::ColMatrix::const_iterator1 sai = a.cm->find1(1, 0,a.col); 202 | FM::ColMatrix::const_iterator1 sai_end = a.cm->find1(1, a.cm->size1(),a.col); 203 | FM::ColMatrix::const_iterator1 sbi = b.cm->find1(1,0, b.col); 204 | while (sai != sai_end) 205 | { 206 | if (*sai < *sbi) 207 | return true; 208 | else if (*sai > *sbi) 209 | return false; 210 | 211 | ++sai; ++sbi; 212 | } ; 213 | return false; // Equal 214 | } 215 | }; 216 | }//namespace 217 | 218 | std::size_t Sample_state_filter::unique_samples () const 219 | /* Count number of unique (unequal value) samples in S 220 | * Implementation requires std::sort on sample column references 221 | */ 222 | { 223 | // Temporary container to Reference each element in S 224 | typedef std::vector SRContainer; 225 | SRContainer sortR(S.size2()); 226 | std::size_t col_index = 0; 227 | for (SRContainer::iterator si = sortR.begin(); si != sortR.end(); ++si) { 228 | (*si).cm = &S; (*si).col = col_index++; 229 | } 230 | // Sort the column proxies 231 | std::sort (sortR.begin(), sortR.end(), ColProxy::less); 232 | 233 | // Count element changes, precond: sortS not empty 234 | std::size_t u = 1; 235 | SRContainer::const_iterator ssi = sortR.begin(); 236 | SRContainer::const_iterator ssp = ssi; 237 | ++ssi; 238 | while (ssi < sortR.end()) 239 | { 240 | if (ColProxy::less(*ssp, *ssi)) 241 | ++u; 242 | ssp = ssi; 243 | ++ssi; 244 | } 245 | return u; 246 | } 247 | 248 | 249 | Sample_filter::Sample_filter (std::size_t x_size, std::size_t s_size) : 250 | Sample_state_filter(x_size,s_size) 251 | /* Initialise filter and set the size of things we know about 252 | * Postcond: s_size >= 1 253 | */ 254 | { 255 | } 256 | 257 | void Sample_filter::predict (Functional_predict_model& f) 258 | /* Predict samples forward 259 | * Pre : S represent the prior distribution 260 | * Post: S represent the predicted distribution 261 | */ 262 | { 263 | // Predict particles S using supplied predict model 264 | const std::size_t nSamples = S.size2(); 265 | for (std::size_t i = 0; i != nSamples; ++i) { 266 | FM::ColMatrix::Column Si(S,i); 267 | FM::noalias(Si) = f.fx(Si); 268 | } 269 | } 270 | 271 | 272 | }//namespace 273 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/BayesFilter/bayesFltAlg.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $Id$ 7 | */ 8 | 9 | /* 10 | * Implement models.hpp : 11 | */ 12 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 13 | #include "bayes_tracking/BayesFilter/matSup.hpp" 14 | #include "bayes_tracking/BayesFilter/models.hpp" 15 | 16 | 17 | namespace { 18 | 19 | template 20 | inline scalar sqr(scalar x) 21 | // Square 22 | { 23 | return x*x; 24 | } 25 | };//namespace 26 | 27 | 28 | /* Filter namespace */ 29 | namespace Bayesian_filter 30 | { 31 | 32 | Simple_additive_predict_model::Simple_additive_predict_model (State_function f_init, const FM::Matrix& G_init, const FM::Vec& q_init) : 33 | // Precondition: G, q are conformantly dimensioned (not checked) 34 | Additive_predict_model (G_init.size1(), q_init.size()), 35 | ff(f_init) 36 | { 37 | G = G_init; 38 | q = q_init; 39 | } 40 | 41 | Simple_linrz_predict_model::Simple_linrz_predict_model (State_function f_init, const FM::Matrix& Fx_init, const FM::Matrix& G_init, const FM::Vec& q_init) : 42 | // Precondition: Fx, G, q are conformantly dimensioned (not checked) 43 | Linrz_predict_model (Fx_init.size1(), q_init.size()), 44 | ff(f_init) 45 | { 46 | Fx = Fx_init; 47 | G = G_init; 48 | q = q_init; 49 | } 50 | 51 | Simple_linear_predict_model::Simple_linear_predict_model (const FM::Matrix& Fx_init, const FM::Matrix& G_init, const FM::Vec& q_init) : 52 | // Precondition: Fx, q and G are conformantly dimensioned (not checked) 53 | Linear_predict_model (Fx_init.size1(), q_init.size()) 54 | { 55 | Fx = Fx_init; 56 | G = G_init; 57 | q = q_init; 58 | } 59 | 60 | Simple_linrz_correlated_observe_model::Simple_linrz_correlated_observe_model (State_function f_init, const FM::Matrix& Hx_init, const FM::SymMatrix& Z_init) : 61 | Linrz_correlated_observe_model (Hx_init.size2(), Hx_init.size1()), 62 | ff(f_init) 63 | /* Linrz observe model initialised from function and model matrices 64 | Precondition: 65 | Hx, Z are conformantly dimensioned (not checked) 66 | */ 67 | { 68 | Hx = Hx_init; 69 | Z = Z_init; 70 | }; 71 | 72 | Simple_linrz_uncorrelated_observe_model::Simple_linrz_uncorrelated_observe_model (State_function f_init, const FM::Matrix& Hx_init, const FM::Vec& Zv_init) : 73 | Linrz_uncorrelated_observe_model (Hx_init.size2(), Hx_init.size1()), 74 | ff(f_init) 75 | /* Linrz observe model initialised from function and model matrices 76 | Precondition: 77 | Hx, Z are conformantly dimensioned (not checked) 78 | */ 79 | { 80 | Hx = Hx_init; 81 | Zv = Zv_init; 82 | }; 83 | 84 | Simple_linear_correlated_observe_model::Simple_linear_correlated_observe_model (const FM::Matrix& Hx_init, const FM::SymMatrix& Z_init) : 85 | Linear_correlated_observe_model (Hx_init.size2(), Hx_init.size1()) 86 | /* Linear observe model initialised from model matrices 87 | Precondition: 88 | Hx, Z are conformantly dimensioned (not checked) 89 | */ 90 | { 91 | Hx = Hx_init; 92 | Z = Z_init; 93 | }; 94 | 95 | Simple_linear_uncorrelated_observe_model::Simple_linear_uncorrelated_observe_model (const FM::Matrix& Hx_init, const FM::Vec& Zv_init) : 96 | Linear_uncorrelated_observe_model (Hx_init.size2(), Hx_init.size1()) 97 | /* Linear observe model initialised from model matrices 98 | Precondition: 99 | Hx, Z are conformantly dimensioned (not checked) 100 | */ 101 | { 102 | Hx = Hx_init; 103 | Zv = Zv_init; 104 | }; 105 | 106 | 107 | 108 | Bayes_base::Float 109 | General_LzUnAd_observe_model::Likelihood_uncorrelated::L(const Uncorrelated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const 110 | /* Definition of likelihood given an additive Gaussian observation model: 111 | * p(z|x) = exp(-0.5*(z-h(x))'*inv(Z)*(z-h(x))) / sqrt(2pi^nz*det(Z)); 112 | * L(x) the the Likelihood L(x) doesn't depend on / sqrt(2pi^nz) for constant z size 113 | * Precond: Observation Information: z,Zv_inv,detZterm 114 | */ 115 | { 116 | if (!zset) 117 | Bayes_base::error (Logic_exception("General_observe_model used without Lz set")); 118 | // Normalised innovation 119 | zInnov = z; 120 | model.normalise (zInnov, zp); 121 | FM::noalias(zInnov) -= zp; 122 | 123 | // Likelihood w of observation z given particular state xi is true state 124 | // The state, xi, defines a predicted observation with a Gaussian 125 | // distribution with variance Zd. Thus, the likelihood can be determined directly from the Gaussian 126 | 127 | FM::Vec::iterator zi = zInnov.begin(), zi_end = zInnov.end(); 128 | for (; zi != zi_end; ++zi) { 129 | *zi *= *zi; 130 | } 131 | Float logL = FM::inner_prod(zInnov, Zv_inv); 132 | 133 | using namespace std; 134 | return exp(Float(-0.5)*(logL + logdetZ)); 135 | } 136 | 137 | void General_LzUnAd_observe_model::Likelihood_uncorrelated::Lz (const Uncorrelated_additive_observe_model& model) 138 | /* Set the observation zz and Zv about which to evaluate the Likelihood function 139 | * Postcond: Observation Information: z,Zv_inv,detZterm 140 | */ 141 | { 142 | zset = true; 143 | // Compute inverse of Zv and its reciprocal condition number 144 | Float rcond = FM::UdUrcond(model.Zv); 145 | model.rclimit.check_PD(rcond, "Z not PD in observe"); 146 | 147 | Bayes_base::Float detZ = 1; 148 | 149 | for (FM::Vec::const_iterator zi = model.Zv.begin(), zi_end = model.Zv.end(); zi != zi_end; ++zi) { 150 | detZ *= *zi; 151 | Zv_inv[zi.index()] = 1 / (*zi); // Protected from /0 by rcond check 152 | } 153 | using namespace std; 154 | logdetZ = log(detZ); // Protected from ln(0) by rcond check 155 | } 156 | 157 | Bayes_base::Float 158 | General_LzCoAd_observe_model::Likelihood_correlated::L(const Correlated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const 159 | /* Definition of likelihood given an additive Gaussian observation model: 160 | * p(z|x) = exp(-0.5*(z-h(x))'*inv(Z)*(z-h(x))) / sqrt(2pi^nz*det(Z)); 161 | * L(x) the the Likelihood L(x) doesn't depend on / sqrt(2pi^nz) for constant z size 162 | * Precond: Observation Information: z,Z_inv,detZterm 163 | */ 164 | { 165 | if (!zset) 166 | Bayes_base::error (Logic_exception ("General_observe_model used without Lz set")); 167 | // Normalised innovation 168 | zInnov = z; 169 | model.normalise (zInnov, zp); 170 | FM::noalias(zInnov) -= zp; 171 | 172 | Float logL = scaled_vector_square(zInnov, Z_inv); 173 | using namespace std; 174 | return exp(Float(-0.5)*(logL + logdetZ)); 175 | } 176 | 177 | void General_LzCoAd_observe_model::Likelihood_correlated::Lz (const Correlated_additive_observe_model& model) 178 | /* Set the observation zz and Z about which to evaluate the Likelihood function 179 | * Postcond: Observation Information: z,Z_inv,detZterm 180 | */ 181 | { 182 | zset = true; 183 | // Compute inverse of Z and its reciprocal condition number 184 | Float detZ; 185 | Float rcond = FM::UdUinversePD (Z_inv, detZ, model.Z); 186 | model.rclimit.check_PD(rcond, "Z not PD in observe"); 187 | // detZ > 0 as Z PD 188 | using namespace std; 189 | logdetZ = log(detZ); 190 | } 191 | 192 | 193 | Bayes_base::Float 194 | General_LzCoAd_observe_model::Likelihood_correlated::scaled_vector_square(const FM::Vec& v, const FM::SymMatrix& V) 195 | /* Compute covariance scaled square inner product of a Vector: v'*V*v 196 | */ 197 | { 198 | return FM::inner_prod(v, FM::prod(V,v)); 199 | } 200 | 201 | 202 | Adapted_Correlated_additive_observe_model::Adapted_Correlated_additive_observe_model (Uncorrelated_additive_observe_model& adapt) : 203 | Correlated_additive_observe_model(adapt.Zv.size()), 204 | unc(adapt) 205 | { 206 | Z.clear(); 207 | for (std::size_t i = 0; i < unc.Zv.size(); ++i) 208 | Z(i,i) = Float(unc.Zv[i]); // ISSUE mixed type proxy assignment 209 | } 210 | 211 | Adapted_Linrz_correlated_observe_model::Adapted_Linrz_correlated_observe_model (Linrz_uncorrelated_observe_model& adapt) : 212 | Linrz_correlated_observe_model(adapt.Hx.size2(), adapt.Hx.size1()), 213 | unc(adapt) 214 | { 215 | Hx = unc.Hx; 216 | Z.clear(); 217 | for (std::size_t i = 0; i < unc.Zv.size(); ++i) 218 | Z(i,i) = Float(unc.Zv[i]); // ISSUE mixed type proxy assignment 219 | } 220 | 221 | 222 | }//namespace 223 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/BayesFilter/covFlt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * See Bayes++.htm for copyright license details 4 | * Copyright (c) 2002 Michael Stevens 5 | * See accompanying Bayes++.htm for terms and conditions of use. 6 | * 7 | * $Id$ 8 | */ 9 | 10 | /* 11 | * Covariance Filter. 12 | */ 13 | #include "bayes_tracking/BayesFilter/covFlt.hpp" 14 | #include "bayes_tracking/BayesFilter/matSup.hpp" 15 | 16 | /* Filter namespace */ 17 | namespace Bayesian_filter 18 | { 19 | using namespace Bayesian_filter_matrix; 20 | 21 | 22 | Covariance_scheme::Covariance_scheme (std::size_t x_size, std::size_t z_initialsize) : 23 | Kalman_state_filter(x_size), 24 | S(Empty), SI(Empty), W(Empty), 25 | tempX(x_size,x_size) 26 | /* Initialise filter and set the size of things we know about 27 | */ 28 | { 29 | last_z_size = 0; // Matrices conform to z_initialsize, they are left Empty if z_initialsize==0 30 | observe_size (z_initialsize); 31 | } 32 | 33 | Covariance_scheme& Covariance_scheme::operator= (const Covariance_scheme& a) 34 | /* Optimise copy assignment to only copy filter state 35 | * Precond: matrix size conformance 36 | */ 37 | { 38 | Kalman_state_filter::operator=(a); 39 | return *this; 40 | } 41 | 42 | 43 | void Covariance_scheme::init () 44 | { 45 | // Postconditions 46 | if (!isPSD (X)) 47 | error (Numeric_exception("Initial X not PSD")); 48 | } 49 | 50 | void Covariance_scheme::update () 51 | { 52 | // Nothing to do, implicit in observation 53 | } 54 | 55 | Bayes_base::Float 56 | Covariance_scheme::predict (Linrz_predict_model& f) 57 | /* Standard Linrz prediction 58 | */ 59 | { 60 | x = f.f(x); // Extended Kalman state predict is f(x) directly 61 | // Predict state covariance 62 | noalias(X) = prod_SPD(f.Fx,X, tempX); 63 | noalias(X) += prod_SPD(f.G, f.q, tempX); 64 | 65 | return 1; 66 | } 67 | 68 | Bayes_base::Float 69 | Covariance_scheme::predict (Gaussian_predict_model& f) 70 | /* Specialised 'stationary' predict, only additive noise 71 | */ 72 | { 73 | // Predict state covariance, simply add in noise 74 | noalias(X) += prod_SPD(f.G, f.q, tempX); 75 | 76 | return 1; 77 | } 78 | 79 | 80 | void Covariance_scheme::observe_size (std::size_t z_size) 81 | /* Optimised dynamic observe sizing 82 | */ 83 | { 84 | if (z_size != last_z_size) { 85 | last_z_size = z_size; 86 | 87 | S.resize(z_size,z_size, false); 88 | SI.resize(z_size,z_size, false); 89 | W.resize(x.size(),z_size, false); 90 | } 91 | } 92 | 93 | Bayes_base::Float 94 | Covariance_scheme::observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s) 95 | /* Correlated innovation observe 96 | */ 97 | { 98 | // Size consistency, z to model 99 | if (s.size() != h.Z.size1()) 100 | error (Logic_exception("observation and model size inconsistent")); 101 | observe_size (s.size());// Dynamic sizing 102 | 103 | // Innovation covariance 104 | Matrix temp_XZ (prod(X, trans(h.Hx))); 105 | noalias(S) = prod(h.Hx, temp_XZ) + h.Z; 106 | 107 | // Inverse innovation covariance 108 | Float rcond = UdUinversePD (SI, S); 109 | rclimit.check_PD(rcond, "S not PD in observe"); 110 | 111 | // Kalman gain, X*Hx'*SI 112 | noalias(W) = prod(temp_XZ, SI); 113 | 114 | // State update 115 | noalias(x) += prod(W, s); 116 | noalias(X) -= prod_SPD(W, S, temp_XZ); 117 | 118 | return rcond; 119 | } 120 | 121 | 122 | Bayes_base::Float 123 | Covariance_scheme::observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s) 124 | /* Uncorrelated innovation observe 125 | */ 126 | { 127 | // Size consistency, z to model 128 | if (s.size() != h.Zv.size()) 129 | error (Logic_exception("observation and model size inconsistent")); 130 | observe_size (s.size());// Dynamic sizing 131 | 132 | // Innovation covariance 133 | Matrix temp_XZ (prod(X, trans(h.Hx))); 134 | noalias(S) = prod(h.Hx, temp_XZ); 135 | for (std::size_t i = 0; i < h.Zv.size(); ++i) 136 | S(i,i) += Float(h.Zv[i]); // ISSUE mixed type proxy assignment 137 | 138 | // Inverse innovation covariance 139 | Float rcond = UdUinversePD (SI, S); 140 | rclimit.check_PD(rcond, "S not PD in observe"); 141 | 142 | // Kalman gain, X*Hx'*SI 143 | noalias(W) = prod(temp_XZ, SI); 144 | 145 | // State update 146 | noalias(x) += prod(W, s); 147 | noalias(X) -= prod_SPD(W, S, temp_XZ); 148 | 149 | return rcond; 150 | } 151 | 152 | }//namespace 153 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/BayesFilter/infFlt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $Id$ 7 | */ 8 | 9 | /* 10 | * Information Filter. 11 | */ 12 | #include "bayes_tracking/BayesFilter/infFlt.hpp" 13 | #include "bayes_tracking/BayesFilter/matSup.hpp" 14 | 15 | /* Filter namespace */ 16 | namespace Bayesian_filter 17 | { 18 | using namespace Bayesian_filter_matrix; 19 | 20 | 21 | Information_scheme::Information_scheme (std::size_t x_size, std::size_t z_initialsize) : 22 | Kalman_state_filter(x_size), Information_state_filter(x_size), 23 | tempX(x_size,x_size), 24 | i(x_size), I(x_size,x_size), 25 | ZI(Empty) 26 | /* Initialise filter and set the size of things we know about 27 | */ 28 | { 29 | last_z_size = 0; // Matrices conform to z_initialsize, they are left Empty if z_initialsize==0 30 | observe_size (z_initialsize); 31 | update_required = true; // Not a valid state, init is required before update can be used 32 | } 33 | 34 | Information_scheme::Linear_predict_byproducts::Linear_predict_byproducts (std::size_t x_size, std::size_t q_size) : 35 | /* Set size of by-products for linear predict 36 | */ 37 | A(x_size,x_size), tempG(x_size,q_size), 38 | B(q_size,q_size), 39 | y(x_size) 40 | {} 41 | 42 | Information_scheme& Information_scheme::operator= (const Information_scheme& a) 43 | /* Optimise copy assignment to only copy filter state 44 | * Precond: matrix size conformance 45 | */ 46 | { 47 | Information_state_filter::operator=(a); 48 | Kalman_state_filter::operator=(a); 49 | return *this; 50 | } 51 | 52 | 53 | void Information_scheme::init () 54 | /* Initialise the filter from x,X 55 | * Precondition: 56 | * x, X 57 | * Postcondition: 58 | * x, X is PD 59 | * y, Y is PSD 60 | */ 61 | { 62 | // Information 63 | Float rcond = UdUinversePD (Y, X); 64 | rclimit.check_PD(rcond, "Initial X not PD"); 65 | // Information state 66 | noalias(y) = prod(Y,x); 67 | update_required = false; 68 | } 69 | 70 | void Information_scheme::init_yY () 71 | /* Initialisation directly from Information 72 | * Precondition: 73 | * y, Y 74 | * Postcondition: 75 | * y, Y is PSD 76 | */ 77 | { 78 | // Postconditions 79 | if (!isPSD (Y)) 80 | error (Numeric_exception("Initial Y not PSD")); 81 | update_required = true; 82 | } 83 | 84 | void Information_scheme::update_yY () 85 | /* Postcondition: 86 | * y, Y is PSD 87 | */ 88 | { 89 | } 90 | 91 | void Information_scheme::update () 92 | /* Recompute x,X from y,Y 93 | * Optimised using update_required (postcondition met iff update_required false) 94 | * Precondition: 95 | * y, Y is PD 96 | * Postcondition: 97 | * x=X*y, X=inv(Y) is PSD 98 | * y, Y is PD 99 | */ 100 | { 101 | if (update_required) 102 | { // Covariance 103 | Float rcond = UdUinversePD (X, Y); 104 | rclimit.check_PD(rcond, "Y not PD in update"); 105 | 106 | noalias(x) = prod(X,y); 107 | update_required = false; 108 | } 109 | } 110 | 111 | Bayes_base::Float 112 | Information_scheme::predict (Linrz_predict_model& f) 113 | /* Extended linrz information prediction 114 | * Computation is through state to accommodate linearised model 115 | */ 116 | { 117 | update (); // x,X required 118 | x = f.f(x); // Extended Kalman state predict is f(x) directly 119 | // Predict information matrix, and state covariance 120 | noalias(X) = prod_SPD(f.Fx,X, tempX); 121 | noalias(X) += prod_SPD(f.G, f.q, tempX); 122 | 123 | // Information 124 | Float rcond = UdUinversePD (Y, X); 125 | rclimit.check_PD(rcond, "X not PD in predict"); 126 | // Predict information state 127 | noalias(y) = prod(Y,x); 128 | return rcond; 129 | } 130 | 131 | Float Information_scheme::predict (Linear_invertable_predict_model& f, Linear_predict_byproducts& b) 132 | /* Linear information predict 133 | * Computation is through information state y,Y only 134 | * Uses x(k+1|k) = Fx * x(k|k) instead of extended x(k+1|k) = f(x(k|k)) 135 | * Requires y(k|k), Y(k|k) 136 | * Predicts y(k+1|k), Y(k+1|k) 137 | * 138 | * The numerical solution used is particularly flexible. It takes 139 | * particular care to avoid invertibility requirements for the noise and noise coupling g,Q 140 | * Therefore both zero noises and zeros in the couplings can be used 141 | */ 142 | { 143 | // A = invFx'*Y*invFx ,Inverse Predict covariance 144 | noalias(b.A) = prod_SPDT(f.inv.Fx, Y, tempX); 145 | // B = G'*A*G+invQ , A in coupled additive noise space 146 | noalias(b.B) = prod_SPDT(f.G, b.A, b.tempG); 147 | for (std::size_t i = 0; i < f.q.size(); ++i) 148 | { 149 | if (f.q[i] < 0) // allow PSD q, let infinity propagate into B 150 | error (Numeric_exception("Predict q Not PSD")); 151 | b.B(i,i) += Float(1) / f.q[i]; 152 | } 153 | 154 | // invert B ,additive noise 155 | Float rcond = UdUinversePDignoreInfinity (b.B); 156 | rclimit.check_PD(rcond, "(G'invFx'.Y.invFx.G + invQ) not PD in predict"); 157 | 158 | // G*invB*G' ,in state space 159 | noalias(Y) = prod_SPD(f.G,b.B, b.tempG); 160 | // I - A* G*invB*G' ,information gain 161 | FM::identity(tempX); 162 | noalias(tempX) -= prod(b.A,Y); 163 | // Information 164 | noalias(Y) = prod(tempX,b.A); 165 | // Information state 166 | noalias(b.y) = prod(trans(f.inv.Fx), y); 167 | noalias(y) = prod(tempX, b.y); 168 | 169 | update_required = true; 170 | return rcond; 171 | } 172 | 173 | 174 | inline void Information_scheme::observe_size (std::size_t z_size) 175 | /* Optimised dynamic observation sizing 176 | */ 177 | { 178 | if (z_size != last_z_size) { 179 | last_z_size = z_size; 180 | 181 | ZI.resize(z_size,z_size, false); 182 | } 183 | } 184 | 185 | Bayes_base::Float 186 | Information_scheme::observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s) 187 | /* Correlated innovation observe 188 | */ 189 | { 190 | // Size consistency, z to model 191 | if (s.size() != h.Z.size1()) 192 | error (Logic_exception("observation and model size inconsistent")); 193 | observe_size (s.size());// Dynamic sizing 194 | 195 | Vec zz(s + prod(h.Hx,x)); // Strange EIF observation object 196 | 197 | // Observation Information 198 | Float rcond = UdUinversePD (ZI, h.Z); 199 | rclimit.check_PD(rcond, "Z not PD in observe"); 200 | 201 | RowMatrix HxT (trans(h.Hx)); 202 | RowMatrix HxTZI (prod(HxT, ZI)); 203 | // Calculate EIF i = Hx'*ZI*zz 204 | noalias(i) = prod(HxTZI, zz); 205 | // Calculate EIF I = Hx'*ZI*Hx 206 | noalias(I) = prod(HxTZI, trans(HxT)); // use column matrix trans(HxT) 207 | 208 | noalias(y) += i; 209 | noalias(Y) += I; 210 | update_required = true; 211 | 212 | return rcond; 213 | } 214 | 215 | Bayes_base::Float 216 | Information_scheme::observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s) 217 | /* Extended linrz uncorrelated observe 218 | */ 219 | { 220 | // Size consistency, z to model 221 | if (s.size() != h.Zv.size()) 222 | error (Logic_exception("observation and model size inconsistent")); 223 | observe_size (s.size());// Dynamic sizing 224 | 225 | Vec zz(s + prod(h.Hx,x)); // Strange EIF observation object 226 | 227 | // Observation Information 228 | Float rcond = UdUrcond(h.Zv); 229 | rclimit.check_PD(rcond, "Zv not PD in observe"); 230 | 231 | RowMatrix HxT (trans(h.Hx)); // HxT = Hx'*inverse(Z) 232 | for (std::size_t w = 0; w < h.Zv.size(); ++w) 233 | column(HxT, w) *= 1 / h.Zv[w]; 234 | // Calculate EIF i = Hx'*ZI*zz 235 | noalias(i) = prod(HxT, zz); 236 | // Calculate EIF I = Hx'*ZI*Hx 237 | noalias(I) = prod(HxT, h.Hx); 238 | 239 | noalias(y) += i; 240 | noalias(Y) += I; 241 | update_required = true; 242 | 243 | return rcond; 244 | } 245 | 246 | }//namespace 247 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/BayesFilter/itrFlt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $ID$ 7 | */ 8 | 9 | /* 10 | * Iterated Covariance Filter. 11 | */ 12 | #include "bayes_tracking/BayesFilter/itrFlt.hpp" 13 | #include "bayes_tracking/BayesFilter/matSup.hpp" 14 | #include "bayes_tracking/BayesFilter/models.hpp" 15 | 16 | /* Filter namespace */ 17 | namespace Bayesian_filter 18 | { 19 | using namespace Bayesian_filter_matrix; 20 | 21 | 22 | bool Counted_iterated_terminator::term_or_relinearize (const Iterated_covariance_scheme& f) 23 | { 24 | --i; 25 | if (i != 0) 26 | m.relinearise (f.x); 27 | return (--i) == 0; 28 | } 29 | 30 | 31 | Iterated_covariance_scheme::Iterated_covariance_scheme(std::size_t x_size, std::size_t z_initialsize) : 32 | Kalman_state_filter(x_size), 33 | S(Empty), SI(Empty), 34 | tempX(x_size,x_size), 35 | s(Empty), HxT(Empty) 36 | /* Initialise filter and set the size of things we know about 37 | */ 38 | { 39 | last_z_size = 0; // Matrices conform to z_initialsize, they are left Empty if z_initialsize==0 40 | observe_size (z_initialsize); 41 | } 42 | 43 | Iterated_covariance_scheme& 44 | Iterated_covariance_scheme::operator= (const Iterated_covariance_scheme& a) 45 | /* Optimise copy assignment to only copy filter state 46 | * Precond: matrix size conformance 47 | */ 48 | { 49 | Kalman_state_filter::operator=(a); 50 | return *this; 51 | } 52 | 53 | void Iterated_covariance_scheme::init () 54 | { 55 | // Postconditions 56 | if (!isPSD (X)) 57 | error (Numeric_exception("Initial X not PSD")); 58 | } 59 | 60 | void Iterated_covariance_scheme::update () 61 | { 62 | // Nothing to do, implicit in observation 63 | } 64 | 65 | Bayes_base::Float 66 | Iterated_covariance_scheme::predict (Linrz_predict_model& f) 67 | { 68 | x = f.f(x); // Extended Kalman state predict is f(x) directly 69 | // Predict state covariance 70 | noalias(X) = prod_SPD(f.Fx,X, tempX); 71 | noalias(X) += prod_SPD(f.G, f.q, tempX); 72 | 73 | return 1; 74 | } 75 | 76 | 77 | void Iterated_covariance_scheme::observe_size (std::size_t z_size) 78 | /* Optimised dynamic observation sizing 79 | */ 80 | { 81 | if (z_size != last_z_size) { 82 | last_z_size = z_size; 83 | 84 | s.resize(z_size, false); 85 | S.resize(z_size,z_size, false); 86 | SI.resize(z_size,z_size, false); 87 | HxT.resize(x.size(),z_size, false); 88 | } 89 | } 90 | 91 | Bayes_base::Float 92 | Iterated_covariance_scheme::observe (Linrz_uncorrelated_observe_model& h, Iterated_terminator& term, const FM::Vec& z) 93 | /* Iterated Extended Kalman Filter 94 | * Bar-Shalom and Fortmann p.119 (full scheme) 95 | * A hard limit is placed on the iterations whatever the 96 | * the normal terminal condition is to guarantee termination 97 | * Uncorrelated noise 98 | */ 99 | { 100 | // ISSUE: Implement simplified uncorrelated noise equations 101 | Adapted_Linrz_correlated_observe_model hh(h); 102 | return observe (hh, term, z); 103 | } 104 | 105 | Bayes_base::Float 106 | Iterated_covariance_scheme::observe (Linrz_correlated_observe_model& h, Iterated_terminator& term, const FM::Vec& z) 107 | /* Iterated Extended Kalman Filter 108 | * Bar-Shalom and Fortmann p.119 (full scheme) 109 | * A hard limit is placed on the iterations whatever the 110 | * the normal terminal condition is to guarantee termination 111 | * returned rcond is of S (or 1 if no iterations are performed) 112 | */ 113 | { 114 | std::size_t x_size = x.size(); 115 | std::size_t z_size = z.size(); 116 | SymMatrix ZI(z_size,z_size); 117 | Matrix HxT(x_size,z_size); 118 | 119 | Vec xpred = x; // Initialise iteration 120 | SymMatrix Xpred = X; 121 | // Inverse predicted covariance 122 | SymMatrix XpredI(x_size,x_size); 123 | Float rcond = UdUinversePD (XpredI, Xpred); 124 | rclimit.check_PD(rcond, "Xpred not PD in observe"); 125 | 126 | // Inverse observation covariance 127 | rcond = UdUinversePD (ZI, h.Z); 128 | rclimit.check_PD(rcond, "Z not PD in observe"); 129 | 130 | RowMatrix HxXtemp(h.Hx.size1(),X.size2()); 131 | RowMatrix temp1(x_size,x_size), temp2(x_size,z_size); 132 | SymMatrix temp3(x_size,x_size); 133 | Vec tempz(z_size); 134 | 135 | do { 136 | // Observation model, linearize about new x 137 | const Vec& zp = h.h(x); 138 | 139 | noalias(HxT) = trans(h.Hx); 140 | // Innovation 141 | h.normalise(s = z, zp); 142 | noalias(s) -= zp; 143 | // Innovation covariance 144 | noalias(S) = prod_SPD(h.Hx, Xpred, HxXtemp) + h.Z; 145 | // Inverse innovation covariance 146 | rcond = UdUinversePD (SI, S); 147 | rclimit.check_PD(rcond, "S not PD in observe"); 148 | 149 | // Iterative observe 150 | noalias(temp3) = prod_SPD(HxT,SI, temp2); 151 | noalias(temp1) = prod(Xpred,temp3); 152 | noalias(X) = Xpred - prod(temp1,Xpred); 153 | 154 | // New state iteration 155 | noalias(temp2) = prod(X,HxT); 156 | noalias(temp1) = prod(X,XpredI); 157 | noalias(tempz) = prod(ZI,s); 158 | x += prod(temp2, tempz) - prod(temp1, (x - xpred)); 159 | } while (!term.term_or_relinearize(*this)); 160 | return rcond; 161 | } 162 | 163 | }//namespace 164 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/BayesFilter/matSup.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Bayes++ the Bayesian Filtering Library 3 | * Copyright (c) 2002 Michael Stevens 4 | * See accompanying Bayes++.htm for terms and conditions of use. 5 | * 6 | * $Id$ 7 | */ 8 | 9 | /* 10 | * Matrix support functions for filter classes 11 | * Relies on Bayesian_filter::Bayes_filter for exception 12 | * thrown by internal matrix checks 13 | */ 14 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" // Exceptions required 15 | #include "bayes_tracking/BayesFilter/matSup.hpp" 16 | #include 17 | #ifndef NDEBUG 18 | #include 19 | #endif 20 | 21 | namespace { 22 | 23 | template 24 | inline scalar sqr(scalar x) 25 | // Square 26 | { 27 | return x*x; 28 | } 29 | };//namespace 30 | 31 | 32 | /* Filter Matrix Namespace */ 33 | namespace Bayesian_filter_matrix 34 | { 35 | 36 | 37 | #ifndef NDEBUG 38 | void assert_isPSD (const SymMatrix &M) 39 | /* Assert a Matrix is Positive Semi Definite via the algorithm in isPSD 40 | * Requires 'cerr' and 'assert' to abort execution 41 | */ 42 | { 43 | bool bPSD = isPSD(M); 44 | if (!bPSD) { 45 | // Display Non PSD 46 | std::cerr.flags(std::ios::scientific); std::cerr.precision(17); 47 | std::cerr << M; 48 | } 49 | assert(bPSD); 50 | } 51 | #endif 52 | 53 | 54 | bool isPSD (const SymMatrix &M) 55 | /* Check a Matrix is both Symetric and Positive Semi Definite 56 | * Creates a temporary copy 57 | * 58 | * Numerics of Algorithm: 59 | * Use UdUfactor and checks reciprocal condition number >=0 60 | * Algorithms using UdUfactor will will therefore succeed if isPSD(M) is true 61 | * Do NOT assume because isPSD is true that M will appear to be PSD to other numerical algorithms 62 | * Return: 63 | * true iff M isSymetric and is PSD by the above algorithm 64 | */ 65 | { 66 | RowMatrix UD(M.size1(),M.size1()); 67 | 68 | RowMatrix::value_type rcond = UdUfactor(UD, M); 69 | return rcond >= 0.; 70 | } 71 | 72 | 73 | bool isSymmetric (const Matrix &M) 74 | /* Check a Symmetric Matrix really is Square and Symmetric 75 | * The later may be implied by the SymMatrix type 76 | * Implicitly also checks matrix is without IEC 559 NaN values as they are always != 77 | * Return: 78 | * true iff M is Square and Symmetric and without NaN 79 | */ 80 | { 81 | // Check Square 82 | if (M.size1() != M.size2() ) { 83 | return false; 84 | } 85 | 86 | // Check equality of upper and lower 87 | bool bSym = true; 88 | std::size_t size = M.size1(); 89 | for (std::size_t r = 0; r < size; ++r) { 90 | for (std::size_t c = 0; c <= r; ++c) { 91 | if( M(r,c) != M(c,r) ) { 92 | bSym = false; 93 | } 94 | } 95 | } 96 | return bSym; 97 | } 98 | 99 | 100 | void forceSymmetric (Matrix &M, bool bUpperToLower) 101 | /* Force Matrix Symmetry 102 | * Normally Copies lower triangle to upper or 103 | * upper to lower triangle if specified 104 | */ 105 | { 106 | // Check Square 107 | if (M.size1() != M.size2() ) { 108 | using namespace Bayesian_filter; 109 | Bayes_base::error (Logic_exception ("Matrix is not square")); 110 | } 111 | 112 | std::size_t size = M.size1(); 113 | 114 | if (bUpperToLower) 115 | { 116 | // Copy Lower to Upper 117 | for (std::size_t r = 1; r < size; ++r) { 118 | for (std::size_t c = 0; c < r; ++c) { 119 | M(c,r) = M(r,c); 120 | } 121 | } 122 | } 123 | else 124 | { 125 | // Copy Upper to Lower 126 | for (std::size_t r = 1; r < size; ++r) { 127 | for (std::size_t c = 0; c < r; ++c) { 128 | M(r,c) = M(c,r); 129 | } 130 | } 131 | } 132 | } 133 | 134 | 135 | 136 | }//namespace 137 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/associationmatrix.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Copyright (C) 2011 by Nicola Bellotto * 3 | * nbellotto@lincoln.ac.uk * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the GNU General Public License as published by * 7 | * the Free Software Foundation; either version 2 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the GNU General Public License * 16 | * along with this program; if not, write to the * 17 | * Free Software Foundation, Inc., * 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 19 | ***************************************************************************/ 20 | #include "bayes_tracking/associationmatrix.h" 21 | 22 | #include "bayes_tracking/BayesFilter/matSup.hpp" 23 | #include 24 | #include 25 | 26 | 27 | AssociationMatrix::AssociationMatrix() 28 | { 29 | RowSize = ColSize = 0; 30 | } 31 | 32 | 33 | AssociationMatrix::AssociationMatrix(size_t row, size_t col) 34 | { 35 | setSize(row, col); 36 | } 37 | 38 | 39 | AssociationMatrix::~AssociationMatrix() 40 | {} 41 | 42 | 43 | void AssociationMatrix::setSize(size_t row, size_t col) 44 | { 45 | if ((row > 0) && (col >= 0)) 46 | { 47 | resize(RowSize = row); 48 | for (size_t i = 0; i < RowSize; i++) (*this)[i].resize(ColSize = col); 49 | } 50 | else 51 | { 52 | cerr << "ERROR! - Wrong size for AssociationMatrix (row = " << row 53 | << ", col = " << col << ")\n"; 54 | exit(1); 55 | } 56 | } 57 | 58 | 59 | void AssociationMatrix::print() 60 | { 61 | for (size_t i = 0; i < RowSize; i++) 62 | { 63 | for (size_t j = 0; j < ColSize; j++) 64 | std::cout << (*this)[i][j] << " "; 65 | std::cout << std::endl; 66 | } 67 | } 68 | 69 | 70 | double AssociationMatrix::mahalanobis(const FM::Vec& v1, 71 | const FM::SymMatrix& R1, 72 | const FM::Vec& v2, 73 | const FM::SymMatrix& R2) 74 | { // sqrt((v1-v2)' * Si * (v1-v2)) 75 | return mahalanobis(Vec(v1-v2), SymMatrix(R1+R2)); 76 | } 77 | 78 | Float AssociationMatrix::detS = 0.; 79 | 80 | double AssociationMatrix::mahalanobis(const FM::Vec& s, 81 | const FM::SymMatrix& S) 82 | { 83 | SymMatrix Si(S.size1(), S.size2()); 84 | Float rcond = UdUinversePD(Si, detS, S); // Si = inv(S) 85 | Numerical_rcond rclimit; 86 | rclimit.check_PD(rcond, "S not PD in AssociationMatrix::mahalanobis(...)"); 87 | return sqrt(inner_prod(trans(s), prod(Si, s))); // sqrt(s' * Si * s) 88 | } 89 | 90 | 91 | double AssociationMatrix::correlation(const FM::Vec& s, const FM::SymMatrix& S) 92 | { 93 | SymMatrix Si(S.size1(), S.size2()); 94 | Float rcond = UdUinversePD(Si, detS, S); // Si = inv(S) 95 | Numerical_rcond rclimit; 96 | rclimit.check_PD(rcond, "S not PD in AssociationMatrix::correlation(...)"); 97 | // exp(-0.5 * (s' * Si * s)) / sqrt(2pi^ns * |S|) 98 | return exp(-0.5*(inner_prod(trans(s),prod(Si, s)))) / sqrt(pow(2*M_PI, (double)s.size())*detS); 99 | } 100 | 101 | double AssociationMatrix::correlation_log(const FM::Vec& s, const FM::SymMatrix& S) 102 | { 103 | SymMatrix Si(S.size1(), S.size2()); 104 | Float rcond = UdUinversePD(Si, detS, S); // Si = inv(S) 105 | Numerical_rcond rclimit; 106 | rclimit.check_PD(rcond, "S not PD in AssociationMatrix::correlation_log(...)"); 107 | // s' * Si * s + ln|S| 108 | return inner_prod(trans(s), prod(Si, s)) + log(detS); 109 | } 110 | 111 | 112 | double AssociationMatrix::gate(int dof) { 113 | switch (dof) { 114 | case 1: // 1 dof @ P=0.01 115 | return 2.576; 116 | break; 117 | case 2: // 2 dof @ P=0.01 118 | return 3.035; 119 | break; 120 | case 3: // 3 dof @ P=0.01 121 | return 3.368; 122 | break; 123 | case 4: // 4 dof @ P=0.01 124 | return 3.644; 125 | break; 126 | default: 127 | cerr << "ERROR - undefined value for gate(" << dof << ")\n"; 128 | exit(EXIT_FAILURE); 129 | } 130 | } 131 | 132 | 133 | void AssociationMatrix::computeNN(measure_t measure) 134 | { 135 | NN.clear(); 136 | URow.clear(); 137 | UCol.clear(); 138 | // fill the vectors containing all the rows and columns 139 | // to be considered for scanning the matrix 140 | for (size_t i = 0; i < RowSize; i++) URow.push_back(i); 141 | for (size_t i = 0; i < ColSize; i++) UCol.push_back(i); 142 | if ((measure == CORRELATION_LOG) || (measure == MAHALANOBIS)) { 143 | // look minimum distances 144 | // loop until the set of selected elements covers all the rows or columns 145 | while (!URow.empty() && !UCol.empty()) { 146 | double min = DBL_MAX; 147 | vector< size_t >::iterator min_ri, riEnd = URow.end(); 148 | vector< size_t >::iterator min_ci, ciEnd = UCol.end(); 149 | // find the matrix element with minimum value 150 | for (vector< size_t >::iterator ri = URow.begin(); ri != riEnd; ri++) { 151 | for (vector< size_t >::iterator ci = UCol.begin(); ci != ciEnd; ci++) { 152 | if ((*this)[*ri][*ci] < min) { 153 | // found new minimum, store it with its indexes 154 | min = (*this)[*ri][*ci]; 155 | min_ri = ri; 156 | min_ci = ci; 157 | } 158 | } 159 | } 160 | // if found a minimum, store its position and remove the relative 161 | // row and column from further consideration 162 | if (min < DBL_MAX) { 163 | match_t p = {*min_ri, *min_ci, min}; 164 | NN.push_back(p); 165 | URow.erase(min_ri); 166 | UCol.erase(min_ci); 167 | } 168 | else // if there are no more minimums, stop looking for 169 | break; 170 | } 171 | } 172 | else if (measure == CORRELATION) { 173 | // look for maximum probabilities 174 | // loop until the set of selected elements covers all the rows or columns 175 | while (!URow.empty() && !UCol.empty()) { 176 | double max = 0.; 177 | vector< size_t >::iterator max_ri, riEnd = URow.end(); 178 | vector< size_t >::iterator max_ci, ciEnd = UCol.end(); 179 | // find the matrix element with maximum value 180 | for (vector< size_t >::iterator ri = URow.begin(); ri != riEnd; ri++) { 181 | for (vector< size_t >::iterator ci = UCol.begin(); ci != ciEnd; ci++) { 182 | if ((*this)[*ri][*ci] > max) { 183 | // found new maximum, store it with its indexes 184 | max = (*this)[*ri][*ci]; 185 | max_ri = ri; 186 | max_ci = ci; 187 | } 188 | } 189 | } 190 | // if found a maximum, store its position and remove the relative 191 | // row and column from further consideration 192 | if (max > 0.) { 193 | match_t p = {*max_ri, *max_ci, max}; 194 | NN.push_back(p); 195 | URow.erase(max_ri); 196 | UCol.erase(max_ci); 197 | } 198 | else // if there are no more maximums, stop looking for 199 | break; 200 | } 201 | } 202 | } 203 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/ekfilter.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Copyright (C) 2006 by Nicola Bellotto * 3 | * nbellotto@lincoln.ac.uk * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the GNU General Public License as published by * 7 | * the Free Software Foundation; either version 2 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the GNU General Public License * 16 | * along with this program; if not, write to the * 17 | * Free Software Foundation, Inc., * 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 19 | ***************************************************************************/ 20 | #include "bayes_tracking/ekfilter.h" 21 | #include 22 | #include 23 | // #include "utility.h" 24 | #include "bayes_tracking/jacobianmodel.h" 25 | 26 | #include "bayes_tracking/BayesFilter/matSup.hpp" 27 | #include "bayes_tracking/BayesFilter/bayesFlt.hpp" 28 | 29 | using namespace Bayesian_filter; 30 | using namespace Bayesian_filter_matrix; 31 | 32 | 33 | EKFilter::EKFilter(std::size_t x_size) : 34 | Kalman_state_filter(x_size), 35 | Covariance_scheme(x_size), 36 | s(Empty), z_p(Empty) 37 | { 38 | EKFilter::x_size = x_size; 39 | FM::Vec x0(x_size); 40 | x0.clear(); 41 | FM::SymMatrix P0(x_size, x_size); 42 | P0.clear(); 43 | init(x0, P0); 44 | } 45 | 46 | 47 | EKFilter::EKFilter(const FM::Vec& x0, const FM::SymMatrix& P0) : 48 | Kalman_state_filter(x0.size()), 49 | Covariance_scheme(x0.size()), 50 | s(Empty), z_p(Empty) 51 | { 52 | EKFilter::x_size = x0.size(); 53 | init(x0, P0); 54 | } 55 | 56 | 57 | EKFilter::~EKFilter() 58 | { 59 | } 60 | 61 | 62 | void EKFilter::init(const FM::Vec& x0, const FM::SymMatrix& P0) 63 | { 64 | init_kalman(x0, P0); 65 | } 66 | 67 | 68 | Bayes_base::Float EKFilter::predict (Linrz_predict_model& f) { 69 | dynamic_cast(f).updateJacobian(x); // update model linearization 70 | return Covariance_scheme::predict(f); 71 | } 72 | 73 | 74 | Bayes_base::Float EKFilter::predict (Gaussian_predict_model& f) { 75 | dynamic_cast(f).updateJacobian(x); // update model linearization 76 | return Covariance_scheme::predict(f); 77 | } 78 | 79 | 80 | void EKFilter::update(Linrz_predict_model& predict_model, 81 | Linrz_correlated_observe_model& observe_model, 82 | const FM::Vec& z) 83 | { 84 | dynamic_cast(predict_model).updateJacobian(x); // update model linearization 85 | predict(predict_model); 86 | 87 | dynamic_cast(observe_model).updateJacobian(x); // update model linearization 88 | observe(observe_model, z); 89 | } 90 | 91 | 92 | void EKFilter::predict_observation(Linrz_correlated_observe_model& observe_model, FM::Vec& z_pred, FM::SymMatrix& R_pred) 93 | { 94 | dynamic_cast(observe_model).updateJacobian(x); // update model linearization 95 | z_pred = observe_model.h(x); // predicted observation 96 | // covariance of predicted observation 97 | Bayesian_filter_matrix::Matrix dum(prod(X, trans(observe_model.Hx))); 98 | noalias(R_pred) = prod(observe_model.Hx, dum); 99 | } 100 | 101 | 102 | Bayes_base::Float EKFilter::observeInnovation(Linrz_correlated_observe_model& h, const Bayesian_filter_matrix::Vec& si, const Bayesian_filter_matrix::SymMatrix& Si) 103 | /* 104 | * Extended linrz correlated observe, compute innovation for observe_innovation 105 | */ 106 | { 107 | dynamic_cast(h).updateJacobian(x); // update model linearization 108 | 109 | if (last_z_size != si.size()) { 110 | s.resize(si.size()); 111 | } 112 | FM::noalias(s) = si; 113 | 114 | // Size consistency, z to model 115 | if (s.size() != h.Z.size1()) 116 | error (Logic_exception("observation and model size inconsistent in observeInnovation")); 117 | observe_size (s.size());// Dynamic sizing 118 | 119 | // Innovation covariance 120 | Bayesian_filter_matrix::Matrix temp_XZ (prod(X, trans(h.Hx))); 121 | noalias(S) = prod(h.Hx, temp_XZ) + h.Z; 122 | 123 | // Inverse innovation covariance 124 | Float rcond = UdUinversePD (SI, S); 125 | rclimit.check_PD(rcond, "S not PD in observeInnovation"); 126 | 127 | // Kalman gain, X*Hx'*SI 128 | noalias(W) = prod(temp_XZ, SI); 129 | 130 | // State update 131 | noalias(x) += prod(W, s); 132 | // update state cov with modified innovation cov 133 | noalias(X) -= prod_SPD(W, Si, temp_XZ); // temp_XZ used just as temporary storage matrix 134 | 135 | return rcond; 136 | } 137 | 138 | 139 | Bayes_base::Float 140 | EKFilter::observe (Linrz_correlated_observe_model& h, const FM::Vec& z) 141 | /* 142 | * Extended linrz correlated observe, compute innovation for observe_innovation 143 | */ 144 | { 145 | dynamic_cast(h).updateJacobian(x); // update model linearization 146 | 147 | Covariance_scheme::update (); 148 | const FM::Vec& zp = h.h(x); // Observation model, zp is predicted observation 149 | 150 | if (last_z_size != z.size()) { 151 | s.resize(z.size()); 152 | } 153 | s = z; 154 | h.normalise(s, zp); 155 | FM::noalias(s) -= zp; 156 | 157 | return observe_innovation (h, s); 158 | } 159 | 160 | 161 | Bayes_base::Float 162 | EKFilter::observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z) 163 | /* 164 | * Extended kalman uncorrelated observe, compute innovation for observe_innovation 165 | */ 166 | { 167 | dynamic_cast(h).updateJacobian(x); // update model linearization 168 | 169 | Covariance_scheme::update (); 170 | const FM::Vec& zp = h.h(x); // Observation model, zp is predicted observation 171 | 172 | if (last_z_size != z.size()) { 173 | s.resize(z.size()); 174 | } 175 | s = z; 176 | h.normalise(s, zp); 177 | FM::noalias(s) -= zp; 178 | return observe_innovation (h, s); 179 | } 180 | 181 | 182 | double EKFilter::logLikelihood() { 183 | SymMatrix Si(S.size1(), S.size2()); 184 | Float detS; 185 | Float rcond = UdUinversePD(Si, detS, S); // Si = inv(S) 186 | Numerical_rcond rclimit; 187 | rclimit.check_PD(rcond, "S not PD in EKFilter::logLikelihood"); 188 | // exp(-0.5 * (s' * Si * s)) / sqrt(2pi^ns * |S|) 189 | return -0.5*(inner_prod(trans(s),prod(Si, s))) - 0.5*((double)s.size()*log(2*M_PI)+log(detS)); 190 | } 191 | -------------------------------------------------------------------------------- /bayestracking/src/bayes_tracking/pfilter.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Copyright (C) 2006 by Nicola Bellotto * 3 | * nbellotto@lincoln.ac.uk * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the GNU General Public License as published by * 7 | * the Free Software Foundation; either version 2 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the GNU General Public License * 16 | * along with this program; if not, write to the * 17 | * Free Software Foundation, Inc., * 18 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 19 | ***************************************************************************/ 20 | #include "bayes_tracking/pfilter.h" 21 | 22 | PFilter::PFilter(std::size_t x_size, std::size_t s_size, SIR_random& random_helper) : 23 | Sample_state_filter (x_size, s_size), 24 | Kalman_state_filter(x_size), 25 | SIR_kalman_scheme(x_size, s_size, random_helper), 26 | w(wir) 27 | { 28 | PFilter::x_size = x_size; 29 | FM::Vec x0(x_size); 30 | x0.clear(); 31 | FM::SymMatrix P0(x_size, x_size); 32 | P0.clear(); 33 | init(x0, P0); 34 | } 35 | 36 | 37 | PFilter::PFilter(std::size_t x_size, std::size_t s_size) : 38 | Sample_state_filter (x_size, s_size), 39 | Kalman_state_filter(x_size), 40 | SIR_kalman_scheme(x_size, s_size, rnd), 41 | w(wir) 42 | { 43 | PFilter::x_size = x_size; 44 | FM::Vec x0(x_size); 45 | x0.clear(); 46 | FM::SymMatrix P0(x_size, x_size); 47 | P0.clear(); 48 | init(x0, P0); 49 | } 50 | 51 | 52 | PFilter::PFilter(const FM::Vec& x0, 53 | const FM::SymMatrix& P0, 54 | std::size_t s_size, 55 | SIR_random& random_helper) : 56 | Sample_state_filter (x0.size(), s_size), 57 | Kalman_state_filter(x0.size()), 58 | SIR_kalman_scheme(x0.size(), s_size, random_helper), 59 | w(wir) 60 | { 61 | PFilter::x_size = x0.size(); 62 | init(x0, P0); 63 | } 64 | 65 | 66 | PFilter::PFilter(const FM::Vec& x0, 67 | const FM::SymMatrix& P0, 68 | std::size_t s_size) : 69 | Sample_state_filter (x0.size(), s_size), 70 | Kalman_state_filter(x0.size()), 71 | SIR_kalman_scheme(x0.size(), s_size, rnd), 72 | w(wir) 73 | { 74 | PFilter::x_size = x0.size(); 75 | init(x0, P0); 76 | } 77 | 78 | 79 | PFilter::~PFilter() 80 | { 81 | } 82 | 83 | 84 | void PFilter::init(const FM::Vec& x0, const FM::SymMatrix& P0) 85 | { 86 | SIR_kalman_scheme::init_kalman(x0, P0); 87 | m_likelihood = 0.; 88 | } 89 | 90 | 91 | void PFilter::predict(Sampled_predict_model& predict_model) 92 | { 93 | SIR_kalman_scheme::predict(predict_model); 94 | update_statistics(); 95 | } 96 | 97 | 98 | void PFilter::predict_observation(Correlated_additive_observe_model& observe_model, FM::Vec& z_pred, FM::SymMatrix& R_pred) 99 | { 100 | z_pred.clear(); // mean 101 | const std::size_t nSamples = S.size2(); 102 | for (std::size_t i = 0; i != nSamples; ++i) { 103 | FM::ColMatrix::Column Si(S,i); 104 | z_pred.plus_assign (observe_model.h(Si)); 105 | } 106 | z_pred /= Float(S.size2()); 107 | 108 | R_pred.clear(); // Covariance 109 | for (std::size_t i = 0; i != nSamples; ++i) { 110 | FM::ColMatrix::Column Si(S,i); 111 | R_pred.plus_assign (FM::outer_prod(observe_model.h(Si)-z_pred, observe_model.h(Si)-z_pred)); 112 | } 113 | R_pred /= Float(nSamples); 114 | } 115 | 116 | 117 | void PFilter::observe(Likelihood_observe_model& observe_model, const FM::Vec& z) 118 | { 119 | SIR_kalman_scheme::observe(observe_model, z); 120 | // keep note of the weight sum (non-normalized likelihood) 121 | const std::size_t nSamples = S.size2(); 122 | m_likelihood = 0; 123 | for (std::size_t i = 0; i != nSamples; ++i) { 124 | m_likelihood += wir[i]; 125 | } 126 | // resample 127 | SIR_kalman_scheme::update_resample(/*Systematic_resampler()*/); 128 | } 129 | 130 | 131 | double PFilter::logLikelihood() { 132 | return log(m_likelihood) - log(S.size2()); // normalized likelihood (S are samples, not covariance) 133 | } 134 | -------------------------------------------------------------------------------- /object3d_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(object3d_detector) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs visualization_msgs geometry_msgs pcl_conversions pcl_ros bayes_tracking) 5 | 6 | find_package(PCL REQUIRED) 7 | 8 | include_directories(include ${catkin_INCLUDE_DIRS}) 9 | 10 | catkin_package() 11 | 12 | catkin_package( 13 | INCLUDE_DIRS include 14 | CATKIN_DEPENDS bayes_tracking) 15 | 16 | ### Offline version 17 | add_executable(object3d_detector src/object3d_detector.cpp src/svm.cpp) 18 | target_link_libraries(object3d_detector ${catkin_LIBRARIES}) 19 | if(catkin_EXPORTED_TARGETS) 20 | add_dependencies(object3d_detector ${catkin_EXPORTED_TARGETS}) 21 | endif() 22 | 23 | ### Online version 24 | add_executable(object3d_detector_ol src/object3d_detector_ol.cpp src/svm.cpp) 25 | target_link_libraries(object3d_detector_ol ${catkin_LIBRARIES}) 26 | if(catkin_EXPORTED_TARGETS) 27 | add_dependencies(object3d_detector_ol ${catkin_EXPORTED_TARGETS}) 28 | endif() -------------------------------------------------------------------------------- /object3d_detector/README.md: -------------------------------------------------------------------------------- 1 | # 3D Object Detector # 2 | 3 | This is a PCL-based ROS package for 3D object detection using 3D LiDARs. 4 | -------------------------------------------------------------------------------- /object3d_detector/config/object3d_detector.yaml: -------------------------------------------------------------------------------- 1 | bayes_people_tracker: 2 | filter_type: "UKF" # The Kalman filter type: EKF = Extended Kalman Filter, UKF = Uncented Kalman Filter 3 | cv_noise_params: # The noise for the constant velocity prediction model 4 | x: 1.4 5 | y: 1.4 6 | std_limit: 1.0 # upper limit for the standard deviation of the estimated position 7 | detectors: # Add detectors under this namespace 8 | object3d_detector: # Name of detector (used internally to identify them). Has to be unique. 9 | topic: "/object3d_detector/poses" # The topic on which the geometry_msgs/PoseArray is published 10 | observation_model: "CARTESIAN" # Obeservation model: CARTESIAN or POLAR 11 | noise_params: # The noise for the cartesian or polar observation model 12 | x: 0.1 13 | y: 0.1 14 | matching_algorithm: "NN" # The algorthim to match different detections. NN = Nearest Neighbour, NNJPDA = NN Joint Probability Data Association 15 | seq_size: 4 # Minimum number of observations for new track creation 16 | seq_time: 0.3 # Minimum interval between observations for new track creation 17 | -------------------------------------------------------------------------------- /object3d_detector/config/object3d_detector_ol.yaml: -------------------------------------------------------------------------------- 1 | bayes_people_tracker_ol: 2 | filter_type: "UKF" # The Kalman filter type: EKF = Extended Kalman Filter, UKF = Uncented Kalman Filter 3 | cv_noise_params: # The noise for the constant velocity prediction model 4 | x: 1.4 5 | y: 1.4 6 | std_limit: 1.0 # upper limit for the standard deviation of the estimated position 7 | detectors: # Add detectors under this namespace 8 | object3d_detector: # Name of detector (used internally to identify them). Has to be unique. 9 | topic: "/object3d_detector_ol/poses" # The topic on which the geometry_msgs/PoseArray is published 10 | observation_model: "CARTESIAN" # Obeservation model: CARTESIAN or POLAR 11 | noise_params: # The noise for the cartesian or polar observation model 12 | x: 0.1 13 | y: 0.1 14 | matching_algorithm: "NN" # The algorthim to match different detections. NN = Nearest Neighbour, NNJPDA = NN Joint Probability Data Association 15 | seq_size: 4 # Minimum number of observations for new track creation 16 | seq_time: 0.3 # Minimum interval between observations for new track creation 17 | -------------------------------------------------------------------------------- /object3d_detector/config/vlp16.yaml: -------------------------------------------------------------------------------- 1 | lasers: 2 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 3 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0, 4 | vert_correction: -0.2617993877991494, vert_offset_correction: 0.0} 5 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 6 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0, 7 | vert_correction: 0.017453292519943295, vert_offset_correction: 0.0} 8 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 9 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0, 10 | vert_correction: -0.22689280275926285, vert_offset_correction: 0.0} 11 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 12 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0, 13 | vert_correction: 0.05235987755982989, vert_offset_correction: 0.0} 14 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 15 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0, 16 | vert_correction: -0.19198621771937624, vert_offset_correction: 0.0} 17 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 18 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0, 19 | vert_correction: 0.08726646259971647, vert_offset_correction: 0.0} 20 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 21 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0, 22 | vert_correction: -0.15707963267948966, vert_offset_correction: 0.0} 23 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 24 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0, 25 | vert_correction: 0.12217304763960307, vert_offset_correction: 0.0} 26 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 27 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0, 28 | vert_correction: -0.12217304763960307, vert_offset_correction: 0.0} 29 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 30 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0, 31 | vert_correction: 0.15707963267948966, vert_offset_correction: 0.0} 32 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 33 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0, 34 | vert_correction: -0.08726646259971647, vert_offset_correction: 0.0} 35 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 36 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0, 37 | vert_correction: 0.19198621771937624, vert_offset_correction: 0.0} 38 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 39 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0, 40 | vert_correction: -0.05235987755982989, vert_offset_correction: 0.0} 41 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 42 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0, 43 | vert_correction: 0.22689280275926285, vert_offset_correction: 0.0} 44 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 45 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0, 46 | vert_correction: -0.017453292519943295, vert_offset_correction: 0.0} 47 | - {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, 48 | focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0, 49 | vert_correction: 0.2617993877991494, vert_offset_correction: 0.0} 50 | num_lasers: 16 51 | 52 | -------------------------------------------------------------------------------- /object3d_detector/include/svm.h: -------------------------------------------------------------------------------- 1 | #ifndef _LIBSVM_H 2 | #define _LIBSVM_H 3 | 4 | #define LIBSVM_VERSION 321 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | extern int libsvm_version; 11 | 12 | struct svm_node 13 | { 14 | int index; 15 | double value; 16 | }; 17 | 18 | struct svm_problem 19 | { 20 | int l; 21 | double *y; 22 | struct svm_node **x; 23 | }; 24 | 25 | enum { C_SVC, NU_SVC, ONE_CLASS, EPSILON_SVR, NU_SVR }; /* svm_type */ 26 | enum { LINEAR, POLY, RBF, SIGMOID, PRECOMPUTED }; /* kernel_type */ 27 | 28 | struct svm_parameter 29 | { 30 | int svm_type; 31 | int kernel_type; 32 | int degree; /* for poly */ 33 | double gamma; /* for poly/rbf/sigmoid */ 34 | double coef0; /* for poly/sigmoid */ 35 | 36 | /* these are for training only */ 37 | double cache_size; /* in MB */ 38 | double eps; /* stopping criteria */ 39 | double C; /* for C_SVC, EPSILON_SVR and NU_SVR */ 40 | int nr_weight; /* for C_SVC */ 41 | int *weight_label; /* for C_SVC */ 42 | double* weight; /* for C_SVC */ 43 | double nu; /* for NU_SVC, ONE_CLASS, and NU_SVR */ 44 | double p; /* for EPSILON_SVR */ 45 | int shrinking; /* use the shrinking heuristics */ 46 | int probability; /* do probability estimates */ 47 | }; 48 | 49 | // 50 | // svm_model 51 | // 52 | struct svm_model 53 | { 54 | struct svm_parameter param; /* parameter */ 55 | int nr_class; /* number of classes, = 2 in regression/one class svm */ 56 | int l; /* total #SV */ 57 | struct svm_node **SV; /* SVs (SV[l]) */ 58 | double **sv_coef; /* coefficients for SVs in decision functions (sv_coef[k-1][l]) */ 59 | double *rho; /* constants in decision functions (rho[k*(k-1)/2]) */ 60 | double *probA; /* pariwise probability information */ 61 | double *probB; 62 | int *sv_indices; /* sv_indices[0,...,nSV-1] are values in [1,...,num_traning_data] to indicate SVs in the training set */ 63 | 64 | /* for classification only */ 65 | 66 | int *label; /* label of each class (label[k]) */ 67 | int *nSV; /* number of SVs for each class (nSV[k]) */ 68 | /* nSV[0] + nSV[1] + ... + nSV[k-1] = l */ 69 | /* XXX */ 70 | int free_sv; /* 1 if svm_model is created by svm_load_model*/ 71 | /* 0 if svm_model is created by svm_train */ 72 | }; 73 | 74 | struct svm_model *svm_train(const struct svm_problem *prob, const struct svm_parameter *param); 75 | void svm_cross_validation(const struct svm_problem *prob, const struct svm_parameter *param, int nr_fold, double *target); 76 | 77 | int svm_save_model(const char *model_file_name, const struct svm_model *model); 78 | struct svm_model *svm_load_model(const char *model_file_name); 79 | 80 | int svm_get_svm_type(const struct svm_model *model); 81 | int svm_get_nr_class(const struct svm_model *model); 82 | void svm_get_labels(const struct svm_model *model, int *label); 83 | void svm_get_sv_indices(const struct svm_model *model, int *sv_indices); 84 | int svm_get_nr_sv(const struct svm_model *model); 85 | double svm_get_svr_probability(const struct svm_model *model); 86 | 87 | double svm_predict_values(const struct svm_model *model, const struct svm_node *x, double* dec_values); 88 | double svm_predict(const struct svm_model *model, const struct svm_node *x); 89 | double svm_predict_probability(const struct svm_model *model, const struct svm_node *x, double* prob_estimates); 90 | 91 | void svm_free_model_content(struct svm_model *model_ptr); 92 | void svm_free_and_destroy_model(struct svm_model **model_ptr_ptr); 93 | void svm_destroy_param(struct svm_parameter *param); 94 | 95 | const char *svm_check_parameter(const struct svm_problem *prob, const struct svm_parameter *param); 96 | int svm_check_probability_model(const struct svm_model *model); 97 | 98 | void svm_set_print_string_function(void (*print_func)(const char *)); 99 | 100 | #ifdef __cplusplus 101 | } 102 | #endif 103 | 104 | #endif /* _LIBSVM_H */ 105 | -------------------------------------------------------------------------------- /object3d_detector/launch/object3d_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /object3d_detector/launch/object3d_detector.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 632 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: Velodyne Cloud 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: false 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: false 51 | - Alpha: 1 52 | Autocompute Intensity Bounds: true 53 | Autocompute Value Bounds: 54 | Max Value: 7.27799 55 | Min Value: -0.939535 56 | Value: true 57 | Axis: Z 58 | Channel Name: intensity 59 | Class: rviz/PointCloud2 60 | Color: 255; 255; 255 61 | Color Transformer: Intensity 62 | Decay Time: 0 63 | Enabled: true 64 | Invert Rainbow: false 65 | Max Color: 255; 255; 255 66 | Max Intensity: 115 67 | Min Color: 0; 0; 0 68 | Min Intensity: 0 69 | Name: Velodyne Cloud 70 | Position Transformer: XYZ 71 | Queue Size: 10 72 | Selectable: true 73 | Size (Pixels): 3 74 | Size (m): 0.02 75 | Style: Flat Squares 76 | Topic: /velodyne_points 77 | Unreliable: false 78 | Use Fixed Frame: true 79 | Use rainbow: true 80 | Value: true 81 | - Class: rviz/MarkerArray 82 | Enabled: true 83 | Marker Topic: /object3d_detector/markers 84 | Name: Detection Markers 85 | Namespaces: 86 | object3d: true 87 | Queue Size: 100 88 | Value: true 89 | - Class: rviz/MarkerArray 90 | Enabled: true 91 | Marker Topic: /people_tracker/marker_array 92 | Name: Tracking Markers 93 | Namespaces: 94 | people_id: true 95 | people_trajectory: true 96 | Queue Size: 100 97 | Value: true 98 | Enabled: true 99 | Global Options: 100 | Background Color: 48; 48; 48 101 | Fixed Frame: velodyne 102 | Frame Rate: 30 103 | Name: root 104 | Tools: 105 | - Class: rviz/Interact 106 | Hide Inactive Objects: true 107 | - Class: rviz/MoveCamera 108 | - Class: rviz/Select 109 | - Class: rviz/FocusCamera 110 | - Class: rviz/Measure 111 | - Class: rviz/SetInitialPose 112 | Topic: /initialpose 113 | - Class: rviz/SetGoal 114 | Topic: /move_base_simple/goal 115 | - Class: rviz/PublishPoint 116 | Single click: true 117 | Topic: /clicked_point 118 | Value: true 119 | Views: 120 | Current: 121 | Class: rviz/Orbit 122 | Distance: 23.3232 123 | Enable Stereo Rendering: 124 | Stereo Eye Separation: 0.06 125 | Stereo Focal Distance: 1 126 | Swap Stereo Eyes: false 127 | Value: false 128 | Focal Point: 129 | X: 0.632407 130 | Y: -0.142279 131 | Z: -0.571869 132 | Name: Current View 133 | Near Clip Distance: 0.01 134 | Pitch: 0.880398 135 | Target Frame: 136 | Value: Orbit (rviz) 137 | Yaw: 4.77358 138 | Saved: ~ 139 | Window Geometry: 140 | Displays: 141 | collapsed: false 142 | Height: 913 143 | Hide Left Dock: false 144 | Hide Right Dock: true 145 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000307fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000307000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001d9000000160000000000000000fb0000000a0049006d00610067006500000002ac000001120000000000000000000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002b3000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000051f0000003efc0100000002fb0000000800540069006d006501000000000000051f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003af0000030700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 146 | Selection: 147 | collapsed: false 148 | Time: 149 | collapsed: false 150 | Tool Properties: 151 | collapsed: false 152 | Views: 153 | collapsed: true 154 | Width: 1311 155 | X: 55 156 | Y: 14 157 | -------------------------------------------------------------------------------- /object3d_detector/launch/object3d_detector_ol.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /object3d_detector/launch/object3d_detector_ol.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.770349 9 | Tree Height: 856 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Velodyne VLP-16 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz/Grid 35 | Color: 160; 160; 164 36 | Enabled: false 37 | Line Style: 38 | Line Width: 0.03 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: false 50 | - Angle Tolerance: 0.1 51 | Class: rviz/Odometry 52 | Color: 255; 25; 0 53 | Enabled: false 54 | Keep: 100 55 | Length: 1 56 | Name: Odometry 57 | Position Tolerance: 0.1 58 | Topic: /pose 59 | Value: false 60 | - Alpha: 1 61 | Class: rviz/RobotModel 62 | Collision Enabled: false 63 | Enabled: false 64 | Links: 65 | All Links Enabled: true 66 | Expand Joint Details: false 67 | Expand Link Details: false 68 | Expand Tree: false 69 | Link Tree Style: Links in Alphabetic Order 70 | Name: P3AT 71 | Robot Description: robot_description 72 | TF Prefix: "" 73 | Update Interval: 0 74 | Value: false 75 | Visual Enabled: true 76 | - Alpha: 1 77 | Autocompute Intensity Bounds: true 78 | Autocompute Value Bounds: 79 | Max Value: 8.18284 80 | Min Value: -0.080926 81 | Value: true 82 | Axis: Z 83 | Channel Name: intensity 84 | Class: rviz/PointCloud2 85 | Color: 255; 255; 255 86 | Color Transformer: FlatColor 87 | Decay Time: 0 88 | Enabled: true 89 | Invert Rainbow: false 90 | Max Color: 255; 255; 255 91 | Max Intensity: 255 92 | Min Color: 0; 0; 0 93 | Min Intensity: 0 94 | Name: Velodyne VLP-16 95 | Position Transformer: XYZ 96 | Queue Size: 10 97 | Selectable: true 98 | Size (Pixels): 3 99 | Size (m): 0.02 100 | Style: Flat Squares 101 | Topic: /velodyne_points 102 | Unreliable: false 103 | Use Fixed Frame: true 104 | Use rainbow: true 105 | Value: true 106 | - Alpha: 1 107 | Autocompute Intensity Bounds: true 108 | Autocompute Value Bounds: 109 | Max Value: 10 110 | Min Value: -10 111 | Value: true 112 | Axis: Z 113 | Channel Name: intensity 114 | Class: rviz/PointCloud2 115 | Color: 255; 255; 255 116 | Color Transformer: Intensity 117 | Decay Time: 0 118 | Enabled: true 119 | Invert Rainbow: false 120 | Max Color: 255; 255; 255 121 | Max Intensity: -999999 122 | Min Color: 0; 0; 0 123 | Min Intensity: 999999 124 | Name: Learned clouds 125 | Position Transformer: XYZ 126 | Queue Size: 100 127 | Selectable: true 128 | Size (Pixels): 3 129 | Size (m): 0.02 130 | Style: Flat Squares 131 | Topic: /object3d_detector_ol/learned_cloud 132 | Unreliable: false 133 | Use Fixed Frame: true 134 | Use rainbow: true 135 | Value: true 136 | - Class: rviz/MarkerArray 137 | Enabled: true 138 | Marker Topic: /object3d_detector_ol/markers 139 | Name: Object3D Detector 140 | Namespaces: 141 | object3d: true 142 | Queue Size: 100 143 | Value: true 144 | - Class: rviz/MarkerArray 145 | Enabled: true 146 | Marker Topic: /people_tracker/marker_array 147 | Name: NB Tracker 148 | Namespaces: 149 | people_id: true 150 | people_trajectory: true 151 | Queue Size: 100 152 | Value: true 153 | - Arrow Length: 0.3 154 | Class: rviz/PoseArray 155 | Color: 255; 25; 0 156 | Enabled: false 157 | Name: Human Trajectory 158 | Topic: /people_tracker/trajectory 159 | Unreliable: true 160 | Value: false 161 | Enabled: true 162 | Global Options: 163 | Background Color: 48; 48; 48 164 | Fixed Frame: odom 165 | Frame Rate: 30 166 | Name: root 167 | Tools: 168 | - Class: rviz/Interact 169 | Hide Inactive Objects: true 170 | - Class: rviz/MoveCamera 171 | - Class: rviz/Select 172 | - Class: rviz/FocusCamera 173 | - Class: rviz/Measure 174 | - Class: rviz/SetInitialPose 175 | Topic: /initialpose 176 | - Class: rviz/SetGoal 177 | Topic: /move_base_simple/goal 178 | - Class: rviz/PublishPoint 179 | Single click: true 180 | Topic: /clicked_point 181 | Value: true 182 | Views: 183 | Current: 184 | Class: rviz/Orbit 185 | Distance: 30.8962 186 | Enable Stereo Rendering: 187 | Stereo Eye Separation: 0.06 188 | Stereo Focal Distance: 1 189 | Swap Stereo Eyes: false 190 | Value: false 191 | Focal Point: 192 | X: 2.44075 193 | Y: -3.53969 194 | Z: 1.74094 195 | Name: Current View 196 | Near Clip Distance: 0.01 197 | Pitch: 0.71848 198 | Target Frame: 199 | Value: Orbit (rviz) 200 | Yaw: 3.89768 201 | Saved: ~ 202 | Window Geometry: 203 | Displays: 204 | collapsed: false 205 | Height: 1056 206 | Hide Left Dock: false 207 | Hide Right Dock: false 208 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000399fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000399000000dd00fffffffb00000016004b006f00640061006b002000530050003300360030000000017e000000f30000000000000000fb00000016005800740069006f006e00200049006d00610067006500000001ca000001090000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f4000000b10000000000000000000000010000025e00000399fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f000000a9fc0100000002fb00000016004b006f00640061006b00200053005000330036003003000002d1000000760000048200000374fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003bfc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 209 | Selection: 210 | collapsed: false 211 | Time: 212 | collapsed: false 213 | Tool Properties: 214 | collapsed: false 215 | Views: 216 | collapsed: false 217 | Width: 1855 218 | X: 65 219 | Y: 24 220 | -------------------------------------------------------------------------------- /object3d_detector/libsvm/README.md: -------------------------------------------------------------------------------- 1 | copy grid.py to ~/.ros/ -------------------------------------------------------------------------------- /object3d_detector/libsvm/pedestrian.range: -------------------------------------------------------------------------------- 1 | x 2 | -1 1 3 | 1 5 10059 4 | 2 0.25 1180.61 5 | 3 0.0228558 1216.5 6 | 4 -323.622 224.904 7 | 5 -113.58 114.963 8 | 6 0.00734402 298.331 9 | 7 -87.6129 30.506 10 | 8 0.000373793 28.0778 11 | 9 2.62097e-08 34807.5 12 | 10 -0.0625267 0.0275383 13 | 11 -0.00202686 0.00626731 14 | 12 0.00133738 61415.8 15 | 13 -0.0110719 0.0466076 16 | 14 0.00145353 92064.89999999999 17 | 15 0 8.418699999999999 18 | 16 0 10.4146 19 | 17 0 7.47525 20 | 18 0 10.3958 21 | 19 0 7.49876 22 | 20 0 10.4562 23 | 21 0 7.55596 24 | 22 0 10.8583 25 | 23 0 7.49553 26 | 24 0 11.0038 27 | 25 0 7.31641 28 | 26 0 10.9144 29 | 27 0 7.30739 30 | 28 0 10.3087 31 | 29 0 7.31104 32 | 30 0 10.2588 33 | 31 0 7.24477 34 | 32 0 10.2172 35 | 33 0 7.18581 36 | 34 0 10.1482 37 | 35 0 5207 38 | 36 0 1063 39 | 37 0 937 40 | 38 0 934 41 | 39 0 768 42 | 40 0 580 43 | 41 0 755 44 | 42 0 1130 45 | 43 0 622 46 | 44 0 401 47 | 45 0 258 48 | 46 0 339 49 | 47 0 327 50 | 48 0 263 51 | 49 0 318 52 | 50 0 263 53 | 51 0 267 54 | 52 0 272 55 | 53 0 300 56 | 54 0 209 57 | 55 0 170 58 | 56 0 137 59 | 57 0 104 60 | 58 0 68 61 | 59 1 291 62 | 60 0 40.855 63 | 61 0 82.9024 64 | -------------------------------------------------------------------------------- /object3d_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | object3d_detector 4 | 0.0.0 5 | 3D object detection in scenes scanned by 3D LiDAR. 6 | Zhi Yan 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | sensor_msgs 13 | visualization_msgs 14 | geometry_msgs 15 | pcl_conversions 16 | pcl_ros 17 | bayes_tracking 18 | 19 | roscpp 20 | sensor_msgs 21 | visualization_msgs 22 | geometry_msgs 23 | pcl_conversions 24 | pcl_ros 25 | bayes_tracking 26 | 27 | --------------------------------------------------------------------------------