|
| 1 | +/** |
| 2 | + * This file is a part of OpenCX, especially about extended Kalman filter (EKF). |
| 3 | + * - Homepage: https://github.com/sunglok/opencx |
| 4 | + */ |
| 5 | + |
| 6 | +/** |
| 7 | + * ---------------------------------------------------------------------------- |
| 8 | + * "THE BEER-WARE LICENSE" (Revision 42): |
| 9 | + * <[email protected]> wrote this file. As long as you retain this notice you |
| 10 | + * can do whatever you want with this stuff. If we meet some day, and you think |
| 11 | + * this stuff is worth it, you can buy me a beer in return. Sunglok Choi |
| 12 | + * ---------------------------------------------------------------------------- |
| 13 | + */ |
| 14 | + |
| 15 | +#ifndef __EKF__ |
| 16 | +#define __EKF__ |
| 17 | + |
| 18 | +#include "opencv2/opencv.hpp" |
| 19 | + |
| 20 | +namespace cx |
| 21 | +{ |
| 22 | + /** |
| 23 | + * @brief Extended Kalman Filter (EKF) |
| 24 | + * |
| 25 | + * The extended Kalman filter is a nonlinear extension of the Kalman filter which linearizes nonlinear state transition and observation models at the point of the current state. |
| 26 | + * You can make your EKF application by inheriting this class and overriding six functions: transitModel, transitJacobian, transitNoise, observeModel, observeJacobian, and observeNoise. |
| 27 | + * |
| 28 | + * @see Extended Kalman Filter (Wikipedia), http://en.wikipedia.org/wiki/Extended_Kalman_filter |
| 29 | + * @see Welch, The Kalman Filter, http://www.cs.unc.edu/~welch/kalman/ |
| 30 | + * @see Welch and Bishop, An Introduction to the Kalman Filter, UNC-Chapel Hill, TR95-041, 2006, <a href="http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html">PDF</a> |
| 31 | + */ |
| 32 | + class EKF |
| 33 | + { |
| 34 | + public: |
| 35 | + /** |
| 36 | + * Initialize the state variable and covariance |
| 37 | + * @param state_dim The dimension of state variable |
| 38 | + * @return True if successful (false if failed) |
| 39 | + */ |
| 40 | + virtual bool initialize(int state_dim, int state_type = CV_64F) |
| 41 | + { |
| 42 | + return initialize(cv::Mat::zeros(state_dim, 1, state_type), cv::Mat::eye(state_dim, state_dim, state_type)); |
| 43 | + } |
| 44 | + |
| 45 | + /** |
| 46 | + * Initialize the state variable and covariance with the given values |
| 47 | + * @param state_vec The given state variable |
| 48 | + * @param state_cov The given state covariance |
| 49 | + * @return True if successful (false if failed) |
| 50 | + */ |
| 51 | + virtual bool initialize(cv::InputArray state_vec, cv::InputArray state_cov = cv::noArray()) |
| 52 | + { |
| 53 | + CV_DbgAssert(!state_vec.empty()); |
| 54 | + state_vec.copyTo(m_state_vec); |
| 55 | + if (m_state_vec.rows < m_state_vec.cols) m_state_vec = m_state_vec.t(); |
| 56 | + CV_DbgAssert(m_state_vec.cols == 1); |
| 57 | + |
| 58 | + int dim = m_state_vec.rows; |
| 59 | + if (!state_cov.empty()) |
| 60 | + { |
| 61 | + state_cov.copyTo(m_state_cov); |
| 62 | + CV_DbgAssert(m_state_cov.rows == dim && m_state_cov.cols == dim); |
| 63 | + } |
| 64 | + else m_state_cov = cv::Mat::eye(dim, dim, m_state_vec.type()); |
| 65 | + return true; |
| 66 | + } |
| 67 | + |
| 68 | + /** |
| 69 | + * Predict the state variable and covariance from the given control input |
| 70 | + * @param control The given control input |
| 71 | + * @return True if successful (false if failed) |
| 72 | + */ |
| 73 | + virtual bool predict(cv::InputArray control) |
| 74 | + { |
| 75 | + // Calculate 'F' and 'Q' |
| 76 | + cv::Mat u = control.getMat(); |
| 77 | + if (u.rows < u.cols) u = u.t(); |
| 78 | + cv::Mat F = transitJacobian(m_state_vec, u); |
| 79 | + cv::Mat Q = transitNoiseCov(m_state_vec, u); |
| 80 | + |
| 81 | + // Predict the state |
| 82 | + m_state_vec = transitModel(m_state_vec, u); |
| 83 | + m_state_cov = F * m_state_cov * F.t() + Q; |
| 84 | + |
| 85 | + // Enforce the state covariance symmetric |
| 86 | + m_state_cov = 0.5 * m_state_cov + 0.5 * m_state_cov.t(); |
| 87 | + return true; |
| 88 | + } |
| 89 | + |
| 90 | + /** |
| 91 | + * Correct the state variable and covariance with the given measurement |
| 92 | + * @param measurement The given measurement |
| 93 | + * @return True if successful (false if failed) |
| 94 | + */ |
| 95 | + virtual bool correct(cv::InputArray measurement) |
| 96 | + { |
| 97 | + // Calculate Kalman gain |
| 98 | + cv::Mat z = measurement.getMat(); |
| 99 | + if (z.rows < z.cols) z = z.t(); |
| 100 | + cv::Mat H = observeJacobian(m_state_vec, z); |
| 101 | + cv::Mat R = observeNoiseCov(m_state_vec, z); |
| 102 | + cv::Mat S = H * m_state_cov * H.t() + R; |
| 103 | + cv::Mat K = m_state_cov * H.t() * S.inv(cv::DecompTypes::DECOMP_SVD); |
| 104 | + cv::Mat innovation = z - observeModel(m_state_vec, z); |
| 105 | + |
| 106 | + // Correct the state |
| 107 | + m_state_vec = m_state_vec + K * innovation; |
| 108 | + cv::Mat I_KH = cv::Mat::eye(m_state_cov.size(), m_state_cov.type()) - K * H; |
| 109 | + //m_state_cov = I_KH * m_state_cov; // The standard form |
| 110 | + m_state_cov = I_KH * m_state_cov * I_KH.t() + K * R * K.t(); // Joseph form |
| 111 | + |
| 112 | + // Enforce the state covariance symmetric |
| 113 | + m_state_cov = 0.5 * m_state_cov + 0.5 * cv::Mat(m_state_cov.t()); |
| 114 | + return true; |
| 115 | + } |
| 116 | + |
| 117 | + /** |
| 118 | + * Calculate squared <a href="https://en.wikipedia.org/wiki/Mahalanobis_distance">Mahalanobis distance</a> of the given measurement |
| 119 | + * @param measurement The given measurement |
| 120 | + * @return The squared Mahalanobis distance |
| 121 | + */ |
| 122 | + virtual double checkMeasurement(cv::InputArray measurement) |
| 123 | + { |
| 124 | + cv::Mat z = measurement.getMat(); |
| 125 | + if (z.rows < z.cols) z = z.t(); |
| 126 | + cv::Mat delta = z - observeModel(m_state_vec, z); |
| 127 | + cv::Mat H = observeJacobian(m_state_vec, z); |
| 128 | + cv::Mat S = H * m_state_cov * H.t(); |
| 129 | + cv::Mat mah_dist2 = delta.t() * S.inv(cv::DecompTypes::DECOMP_SVD) * delta; |
| 130 | + return cv::sum(mah_dist2)(0); |
| 131 | + } |
| 132 | + |
| 133 | + /** |
| 134 | + * Assign the state variable with the given value |
| 135 | + * @param state The given state variable |
| 136 | + * @return True if successful (false if failed) |
| 137 | + */ |
| 138 | + bool setState(cv::InputArray state) |
| 139 | + { |
| 140 | + CV_DbgAssert(state.size() == m_state_vec.size()); |
| 141 | + CV_DbgAssert(state.type() == m_state_vec.type()); |
| 142 | + m_state_vec = state.getMat(); |
| 143 | + return true; |
| 144 | + } |
| 145 | + |
| 146 | + /** |
| 147 | + * Get the current state variable |
| 148 | + * @return The state variable |
| 149 | + */ |
| 150 | + const cv::Mat getState() { return m_state_vec; } |
| 151 | + |
| 152 | + /** |
| 153 | + * Assign the state covariance with the given value |
| 154 | + * @param covariance The given state covariance |
| 155 | + * @return True if successful (false if failed) |
| 156 | + */ |
| 157 | + bool setStateCov(const cv::InputArray covariance) |
| 158 | + { |
| 159 | + CV_DbgAssert(covariance.size() == m_state_cov.size()); |
| 160 | + CV_DbgAssert(covariance.type() == m_state_cov.type()); |
| 161 | + m_state_cov = covariance.getMat(); |
| 162 | + return true; |
| 163 | + } |
| 164 | + |
| 165 | + /** |
| 166 | + * Get the current state covariance |
| 167 | + * @return The state covariance |
| 168 | + */ |
| 169 | + const cv::Mat getStateCov() { return m_state_cov; } |
| 170 | + |
| 171 | + protected: |
| 172 | + /** |
| 173 | + * The state transition function |
| 174 | + * @param state The state variable |
| 175 | + * @param control The given control input |
| 176 | + * @return The predicted state variable |
| 177 | + */ |
| 178 | + virtual cv::Mat transitModel(const cv::Mat& state, const cv::Mat& control) = 0; |
| 179 | + |
| 180 | + /** |
| 181 | + * Return the Jacobian of the state transition function |
| 182 | + * @param state The state variable |
| 183 | + * @param control The given control input |
| 184 | + * @return The Jacobian of the state transition function |
| 185 | + */ |
| 186 | + virtual cv::Mat transitJacobian(const cv::Mat& state, const cv::Mat& control) = 0; |
| 187 | + |
| 188 | + /** |
| 189 | + * Return the state transition noise |
| 190 | + * @param state The state variable |
| 191 | + * @param control The given control input |
| 192 | + * @return The state transition noise |
| 193 | + */ |
| 194 | + virtual cv::Mat transitNoiseCov(const cv::Mat& state, const cv::Mat& control) = 0; |
| 195 | + |
| 196 | + /** |
| 197 | + * The state observation function |
| 198 | + * @param state The state variable |
| 199 | + * @param measurement The given measurement |
| 200 | + * @return The expected measurement |
| 201 | + */ |
| 202 | + virtual cv::Mat observeModel(const cv::Mat& state, const cv::Mat& measurement) = 0; |
| 203 | + |
| 204 | + /** |
| 205 | + * Return the Jacobian of the state observation function |
| 206 | + * @param state The state variable |
| 207 | + * @param measurement The given measurement |
| 208 | + * @return The Jacobian of the state observation function |
| 209 | + */ |
| 210 | + virtual cv::Mat observeJacobian(const cv::Mat& state, const cv::Mat& measurement) = 0; |
| 211 | + |
| 212 | + /** |
| 213 | + * Return the state observation noise |
| 214 | + * @param state The state variable |
| 215 | + * @param measurement The given measurement |
| 216 | + * @return The state observation noise |
| 217 | + */ |
| 218 | + virtual cv::Mat observeNoiseCov(const cv::Mat& state, const cv::Mat& measurement) = 0; |
| 219 | + |
| 220 | + /** The state variable */ |
| 221 | + cv::Mat m_state_vec; |
| 222 | + |
| 223 | + /** The state covariance */ |
| 224 | + cv::Mat m_state_cov; |
| 225 | + }; // End of 'EKF' |
| 226 | + |
| 227 | +} // End of 'cx' |
| 228 | + |
| 229 | +#endif // End of '__EKF__' |
0 commit comments