example implementation

[sesovero]

I was hoping to run your algorithm using the rosbag data files posted (http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:apartment:home#raw_data). I have figured how to route some of the messages such as the IMU(https://dl.dropbox.com/u/36065/Screenshot%20from%202012-08-21%2015%3A02%3A33.png). I was uncertain of where the actual Kalman filtering is occurring and was wondering if you could place an example RX graph in the repository or wiki article. Thanks.

[stephanweiss]

Hi,

Indeed, the tutorials are still missing, we are working on this and put them online ASAP.

The filter is divided into 2 parts: first, ssf_core containing the core-part containing the prediction step and the general update steps. That is "where the actual Kalman filtering is occurring". Second, ssf_updates contains the construction of the sensor-specific update matrices and residuals.

The current framework is made to fuse a single sensor with IMU readings. Hence, you want to start only the position_sensor (if you have a 3DoF position sensor) OR pose_sensor (if you have a 6DoF pose sensor) module.

Let us assume you want to use the GPS position data and fuse it with the IMU data of the rosbag data file you mentioned. Then you need the following steps running the position_sensor node:

1. Convert the GPS position (/position_gps) into x,y,z coordinates of your (global) navigation coordinate frame, for example into /gps_xyz

2. map /gps_xyz to /totalstation/position. This is the measurement sensor (i.e. for the Kalman filter update step)

3. map /imu_data to /ssf_core/imu_state_input. This is the prediction sensor (i.e. for the Kalman filter propagation step)

4. GPS is usually not very precise. We suggest to move the platform quite a bit (+/- 10m in each axis several times) to make the filter converge correctly. Also, the correct yaw initialization is crucial to not converge into a local minimum.

I hope this helps, let me know if you need more details.

Best
Stephan

[stephanweiss]

Hi again,

I was just told that the data set you are using was taken during a stationary phase. Note that the filter framework needs excitations on the accelerometers and the gyroscopes in order to be fully observable. Thus, the data set you are using is not a good sample.
We upload the tutorials and an adequate data set ASAP

Best
Stephan

[sesovero]

Thanks so much for all this information. I have been able to implement the
rerouting of signals that you suggested. I am unclear on where to read the
state estimation of the Kalman filter.

What is the use of :

"/mav1/fcu/ekf_state_in" and "/mav1/fcu/ekf_state_out" by the pose and
position sensor is unclear to me.

Additionally, I am unclear what node actually publishes the kalman filter's
(pose and position) state estimate, or do they maintain their own estimate
separately. If so what how do they need to communicate with each other to
share information?

Sebastian

On Mon, Aug 27, 2012 at 7:35 AM, stephanweiss notifications@github.comwrote:

Hi again,

I was just told that the data set you are using was taken during a
stationary phase. Note that the filter framework needs excitations on the
accelerometers and the gyroscopes in order to be fully observable. Thus,
the data set you are using is not a good sample.
We upload the tutorials and an adequate data set ASAP

Best
Stephan

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/3#issuecomment-8057503.

[stephanweiss]

Sebastian,

ssf_core/correction (from ssf_core): this is the output message from the filter framework containing the calculated state correction vector. This is used if you propagate the EKF states externally (e.g. on the high-level processor of a helicopter of Ascending Technologies, cf asctec_mav:framework ROS stack) such that the external propagator can apply the corrections.
If the "initialize" flag is set in the message, this vector contains the state, not the corrections, and can be used to initialize the external propagator. The flag is set once upon filter initialization.

fcu/ekf_state_in (from asctec_mav_framework): this is the correction input for the EKF propagation on the high-level (HL) processor on the Ascending Technologies (AscTec) hard-ware (ARM7). If you use an AscTec helicopter, their latest firmware and the asctec_mav_framework ROS stack, this input message works as is with ssf_core/correction. If the "initialize" flag is set in the message, the propagator takes the values as initial state, not as corrections to be applied to its own propagated state.

fcu/ekf_state_out (from asctec_mav_framework): this is the state (6DoF pose and 3DoF velocity) predicted by the external propagator including the current IMU readings. You want to use this for MAV control if you the ascec_mav_framework ROS stack.

ssf_core/hl_state_input (from ssf_core): prediction callback for the filter framework. Here, the state is assumed to be propagated externally, the ssf_core only propagates the covariance matrix upon reception of a hl_state_input message and stores the received state for potential update callback call [1].

If you do not use an external state propagator, you should use the ssf_core/imu_state_input message instead of the ssf_core/hl_state_input message. Then, the state and covariance matrix are both propagated within the filter framework.

In any case you have two messages providing the current pose at the rate the state prediction is called (i.e. IMU rate):
ssf_core/pose: 6DoF pose with covariance stamped
ssf_core/ext_state: legacy message for the Luenberger controller on the AscTec hardware: 6DoF pose, 3DoF velocity, no covariances

The full state is available at update rate on ssf_core/state_out (cf state.h for the mapping of the different states)

hope this helps, best
Stephan

[stephanweiss]

forgot the citation:
[1] Stephan Weiss, Markus W. Achtelik, Margarita Chli and Roland Siegwart. "Versatile Distributed Pose Estimation and Sensor Self-Calibration for Autonomous MAVs". in IEEE International Conference on Robotics and Automation (ICRA), 2012.