Autoware.Auto
|
|
We need a node for state estimation within Autoware Auto. This node uses kalman_filter
package as the backbone and provides interface similar to the robot_localization package, albeit following a different design.
For now we assume that tracking happens in 2D and that the Constant Acceleration motion model is used. The code is designed in a way to allow for configuring these at a later point in time.
Note that the outgoing messages will be timestamped in the same time reference frame as the incoming messages, and we assume that all clocks that timestamp the incoming messages are properly synchronized.
The inputs are measurements that update the prediction of the underlying EKF estimate. Currently, the node supports the following inputs:
geometry_msgs/msg/PoseWithCovariance
- updates the 2d positiongeometry_msgs/msg/TwistWithCovariance
- updates the 2d speednav_msgs/msg/Odometry
- updates both position and speedThe state estimator provides the following output:
nav_msgs/msg/Odometry
on the topic filtered_state
that can be remapped by the user.Note: we will only focus on time-stamped messages here.
The core functionality of this node resides in the KalmanFilterWrapper
class. To initialize this class we need the following:
This class provides a high-level interface to use potentially different Kalman Filters implementations under the hood, configuring them through the template parameters of this class. It supports all the classical operations of the Kalman Filter such as prediction, update (in this case from ROS messages) as well as getting the state and its covariance as a ROS message.
Note: The filter will not predict the state before it has seen a stateful observation. After that it works as intended.
Just as a short recap, following this discussion.
For a state of position, velocity and acceleration we have the following representation of the transition funtion and state vector (in a 1D case):
\[ F = \left[\begin{matrix}1 & dt & \frac{dt^{2}}{2}\\0 & 1 & dt\\0 & 0 & 1\end{matrix}\right],\hspace{5mm}x = \left[\begin{matrix}x\\v\\a\end{matrix}\right] \]
The temporal update to the state covariance looks like this (I am assuming that the \(Q\) matrix does not change with time for simplicity of notation):
\[ P_t = F P_{t-1} F^\top + G Q G^\top \]
The parts that might get confusing:
\[ G \cdot Q \cdot G^\top = \left[\begin{matrix}\frac{dt^{2}}{2}\\dt\\1\end{matrix}\right] \cdot \sigma^2_a \cdot \left[\begin{matrix}\frac{dt^{2}}{2} & dt & 1\end{matrix}\right] = \left[\begin{matrix}\frac{dt^{4}}{4} & \frac{dt^{3}}{2} & \frac{dt^{2}}{2}\\\frac{dt^{3}}{2} & dt^{2} & dt\\\frac{dt^{2}}{2} & dt & 1\end{matrix}\right] \cdot \sigma^2_a \]
This matrix has a zero determinant and cannot be factorized, so the description for ourGQ_chol
variable is wrong as it is not a Cholesky factor. It is just such a matrix/vector that multiplied by itself transposed gives us some matrix that represents the process noise.