# Can i use a predictive kalman filter to 'increase' my sample rate?

I have a slam algorithm that outputs at around 30Hz, an implementation of ORBSLAM2.

https://github.com/raulmur/ORB_SLAM2

I am reading this into a renderer that expects 60+ Hz.

Because my sample speed is low, I am getting 'shuddering' in the display, as the renderer adds linear 'steps' between the samples.

For example, I am seeing a result like:

time   sample    result

1         20          20
2         n/a         20
3         n/a         20
4         22          22
5         n/a         22
6         n/a         22
7         24          24
8         n/a         24
9         n/a         24


What i need to do, is predict the next sample, and fill in the gaps, so to speak, so that I end up with something like:

time   sample    result

1         20          20
2         n/a         20.66
3         n/a         21.33
4         22          22
5         n/a         22.66
6         n/a         23.66
7         24          24
8         n/a         24.33
9         n/a         25.66


I need to predict 6DOF, for which i have translation xyz, and a quaternion xyzw. But if I can find a way to predict even one axis, for a start, that would be great.

I have the data outputting as xyz and xyzw, at around 30Hz. I also have an xsens IMU, which i am using to pass in an initial rotation value.

Can i use a predictive filter for this purpose? Is a kalman suitable?

I am looking at:

https://github.com/simondlevy/TinyEKF

and a Bayes filter:

http://bayesclasses.sourceforge.net/Bayes++.html

But am a little out of my depth.

• What type of SLAM are you using? Also, what are the states you need to "fill in"? Are you trying to predict pose, landmarks, point clouds? – Ralff Apr 3 '17 at 1:15
• Hi, thanks for your reply. I have added more info. I need to predict camera pose as translation xyz and quaternion rotation. It is an implementation of ORBSLAM2. – anti Apr 3 '17 at 8:55
• ... But for now, if I could get even a single value predicting as i want, i can extrapolate from there! – anti Apr 3 '17 at 11:06

In this situation, you probably shouldn't use a Kalman filter for "filling" in the gaps. Typically, you use a Kalman filter (or Bayes filter etc...) to fuse information from different sources.

In your case, you have are using SLAM for localization. The SLAM algorithm is taking in data and estimating the pose of the camera as well as a sparse 3D reconstruction of the environment. Internally, this probably already performs some type of prediction/update for tracking etc...

The problem you are having is that the algorithm is only running at 30Hz, and you need 60Hz of pose data if I understand correctly. If I were you, I would take the states (position and velocity) at each time step and propagate them forward (at 60Hz).

To propagate them forward, I mean integrate your pose using your previous state estimates. For example, if your states were x position, y position, x velocity, and y velocity, then you can assume your velocity is constant and propagate your position using that velocity.

$$x_{k+1} = x_k + v_{x,k} \Delta t$$

$$y_{k+1} = y_k + v_{y,k} \Delta t$$

Note that you will need to write out the equations for your system. In the above equations, the $k$ is the previous time step, and $\Delta t$ is the change in time between the previous time step and the current time step. Once you receive another measurement from the SLAM algorithm, use that measurement instead of the propagated state, then repeat. You can keep the intermediate propagated states, but you don't want to perform an update because your propagated states are not based on sensor measurements. So, basically in between the measurements you get from ORBSLAM, you will have these intermediate measurements.

Edit: Keep in mind that the question you asked is about how to fill in the gaps given low frequency data. I am assuming you do not have extra sensors such as IMU, GPS, encoders, etc... Regardless, if additional sensors are available, this still doesn't address how to obtain data at a higher frequency (although it is likely the IMU will operate above 50Hz). I am suggesting an approach to fill in missing data without adding complexity to your system. Assuming constant velocity is reasonable if the motion isn't highly dynamic. If you do have additional sensors you want to incorporate, I think you would want to reconsider your overall approach because you can use IMU and encoders to improve the feature tracking and reconstruction.

• Thanks! Can you please explain what you mean by propagate them forward (at 60Hz)  ? I need to avoid the 'stepping' as above, so i cannot just duplicate the same value until i get a new updated value. – anti Apr 3 '17 at 16:39
• @anti, the 'k' you see is a second subscript on the velocity variable. You want (new x value) = (current x value) + (current velocity) * (time difference). NOTE: To be most correct you should also propagate for the covariance matrices for the save time difference. If you do this then you can simply apply the measurement when it arrives and it will 'just work'. If you don't want to do that, or it's too confusing, you need to remember all the variables from the last measurement step (i.g. ignore any variables from the intermediate propagation) – ryan0270 Apr 3 '17 at 20:42
• Sorry, that sentence was horrible (fingers not typing what I told them). It should read "propagate the covariance matrices for the same time difference". I'm not sure what source you're using for your Kalman filter, but using en.wikipedia.org/wiki/Kalman_filter as a reference I'm talking about the second equation under the Details::Predict section (the first equation is just the x update equation you already have now, but in a fancied up form) – ryan0270 Apr 3 '17 at 21:09
• No, I mean when you get a new measurement you need to throw away all the intermediate steps before applying the new measurement. This means using all the old variable values you had immediately after the last measurement. – ryan0270 Apr 3 '17 at 21:18
• Please see my additional edits. I still do not recommend a Kalman filter for this application. You do not need to propagate covariance since you would just be filling in the gaps. You are not modifying the 30Hz solution. – Ralff Apr 3 '17 at 21:37

Short answer, yes, if you use your IMU to fill in the gaps.

If you're using ROS, this is a great use-case for the robot_localization package.