# Velocity from Sensor Data - Best Approach?

I am attempting to develop a control system based upon rocket velocity. My first step is attempting to properly filter my sensor data in order to calculate velocity.

What is the best approach to get accurate, smooth, and fast data to my control logic?

Would I benefit from combining accelerometer and barometric data? If so, how?

Background:

I am currently only using a BMP180 barometer to get altitude and then calculating velocity from the last altitude measurement. I am operating the BMP180 on ultra low power to increase the sample rate, but that comes at a cost of a RMS noise of around 0.5 meters.

Using 1D Kalman filtering I can get it smoothed out somewhat (tried on both velocity and altitude), but it is still not anywhere near as smooth as I need for my control system. It also lags behind the data far too much when I get it smoothed.

I have attached an example of what I have done with Kalman filtering. The original data is from a Featherweight Raven altimeter. I used the reported altitude data to calculate velocity (standard deltX/deltT) for each iteration, filtered it, and compared it with the Raven's own calculated velocity from it's accelerometer. The altitude data is at 20 Hz which is about what I am achieving with my Arduino setup right now.

Thanks!

• First off, this is cool. When I first read your question, I was going to say that this is by no means easy, but the fact that you know what a Kalman filter removed any doubts entirely. In any case, I have a few ideas, but questions first! (1) How smooth do you want your data? It looks relatively smooth to me, but I can't say I know the sensitivity of your control system. (2) What was your impetus to use a Kalman filter? (3) How did you implement the filter? (4) Do you have a gyroscope, (5) Could you post your code? Commented Jul 1, 2015 at 23:46
• Thanks for the reply! I am not too familiar with the Kalman filter, but I do have an aerospace engineering background. 1) For example, in this case, the rocket's velocity should only be affected by air drag and gravity. There should be no spikes down or especially up like in the data I tested against. I would like to see a curve similar to the grey line, but it seems to lead rather than follow the as the Kalman filter is causing. 2) I have read about it being used in similar control systems such as robotics (which I consider this very similar to). 3) It is a very simple implementation..... Commented Jul 2, 2015 at 1:32
• ...I wanted to initially just filter the sensor data versus model the system (which the Kalman filter can do) so I opted for the simplest approach. 4) I do have a gyroscope (L3GD20H) and accelerometer/compass (LSM303) from the Adafruit 10DOF. 5) I do not currently have the filtering implemented in my code, I tweaked the parameters and tested it using my data output and excel (and in the case of the screenshot, previous rocket flight data from another altimeter). My steady state parameters... Commented Jul 2, 2015 at 1:41
• ... from the Kalman filter implementation I posted are as follows: P: 27.92 or 22.92 (depending upon where in the code) K: 0.1791 R: 128 Q: 5 @Mlagma The reason I haven't moved onto using a matrix implementation of the Kalman filter yet, is that I am worried it may still produce significant lag and I wanted to know if there was a better filtering technique for what I am doing. Commented Jul 2, 2015 at 1:41
• I just wanted to thank you for making this post. I had a similar question.
– r3wt
Commented Jan 4, 2016 at 10:04

Welcome to the world of signal processing! (Starting off with a Kalman filter, though, is not very welcoming).

The first few things to address are in regard to the barometer:

(1) Inevitably, it's going to be noisy, and barometers in particular are known to be noisy. Considering the unfiltered data you posted, I'd say it's reasonable. The spikes are completely normal, especially being that your calculating the derivative from the barometer data. If you want to try to smooth the barometer data before passing it to the Kalman filter, you could apply a nth order low pass filter to the sampled data. This, of course, would add more lag to your calculations - it's really a trade off.

(2) Looking at the unfiltered data and the velocity calculated from the Raven, it appears that there might be an issue. Starting at time 0, the unfiltered data and the Raven data is nearly identical. Once the rocket reaches 180ft/s, though, the filtered data appears to diverge. After it passes the global maxima and hits 180ft/s again, the two data outputs start to converge. This makes me suspect that your sampling rate may be too low. A hard-core, by the book, DSP engineer will probably start crying when he reads this, but consider this: at a velocity of 180ft/s with a sample rate of 20S/s, each sample corresponds to a change in position of 9ft. So, at best, you have a resolution of 9ft per second. This can pose at least two problems. (1) The barometer has to continuously update temperature and pressure readings in order to calculate an accurate altitude. I only glanced at the datasheet, but it seems like the temperature (and I think one other parameter) is updated at a slower rate than the pressure sampling - that is, there's an oversampling factor for the pressure measurement. Hence, your calculation may start to diverge from reality. And as the rocket's velocity increases, this problem may be exacerbated. Of course, temperature changes at rate of about 9.8C/1000m, so it may not be that significant of an issue. Just wanted to throw it out there. (2) Again, at 180ft/s with a 20Hz sampling rate, you basically have a resolution of 9ft/S. This isn't necessarily bad, but one thing to keep in mind is that a Kalman filter is predictive; it predicts the state of the system 1/20 of a second into the future. If your underlying model is off, this could lead to slower convergence and larger corrections that would make the filtered data look "jerky."

Just one more thing before we get into the filter...

It's worth considering how you're implementing this on the Arduino. There shouldn't be any issues with computation time that could result in significant lag. If your only sampling the altitude at 20Hz and then feeding it to the filter at the same speed, you should be good to go. The only issue I can foresee is if you're using the Arduino Libraries. If you are, you'll probably run into bottlenecks and missed deadlines.

Anyway, the primary issue you're having - in my opinion - is not related to computational speed or hardware issues. Instead, I suspect it's your implementation of the filter.

One thing that stood out to me were your values of R and Q. R is the measurement noise covariance, and Q is the process noise covariance. Covariance ranges between [-1, 1]. Your Q = 5. I quickly scanned the link you provided, so I might have missed a simplification they did. I've seen in some cases that the covariance is simplified to a proportional expectation value and left unnormalized, similar to what's seen in Gram matrices.

Regardless, here's some insight that may be useful:

The Kalman filter is very much dependent on how you model your system.

To illustrate this, let's consider an example analogous to your rocket. (I came across this example at some point, but I don't recall the source):

Say you want to measure the water level in a bucket, but you have a sensor that gives you noisy data. And of course, to filter the data, you decide to use a 1-D Kalman filter.

We assume that we have a pretty accurate model of our system, so we'll let Q = 0.001. We iterate through the filter, and we may get data that looks like this:

The filter works fairly well.

But what if our model happened to be off - that is, it doesn't reflect what's really happening?

In the image above, the water level was constant. In reality, perhaps the tank was filling at a constant rate. What happens now if we use the same filter?

Clearly, the filter is diverging. The lag, as you call it, isn't really lag or a delay in the sense that the computation is slow. (In some ways it is a delay, but only in the mathematical sense that the filter is slowly converging). The "lag" effect we're seeing is due to our model. In essence, the filter we designed thinks our model is so good (as indicated by our low Q) that it gives greater weight to our model of the system. As you probably guessed, one way we can fix this problem is by increasing Q. So, let Q = 0.1:

You can keep increasing Q, and when Q = 1, your filtered data should actually match your measurement data.

This, though, really ins't the best way. Ideally, you need to start with a relatively accurate model. Q can be thought of as a measure of the error in your model, and you want to minimize error. At this point, you'll inevitable need to start work with matrices. This won't increase "lag." In fact, it should help your filtered data converge quicker.

There is one more thing to note. Kalman filters prefer linear data (speaking generally here). Look what happens when you feed in nonlinear data:

To accurately and effectively apply a Kalman filter to non-linear data without such pronounced "lag", you need to use what is known as an extended Kalman filter - which basically linearizes your data.

Before you go down that route, which would probably be overkill, I would suggest improving your model used in your Kalman filter to better reflect it's true state. This means ditching the simplified filter you found and going into the matrix math.

In any case, I hope this helped. Feel free to ask any questions.

(Another method just crossed my mind that may be a simpler approach than a Kalman filter. I'll add it tomorrow).

• Thanks for the awesome info! I want to get into the Kalman filter modeling for the actual rocket launch, but I don't have any unfiltered data. The Raven already filters its data and I can't test against that. I have been messing around with combining the low pass filter with the 1DKalman I was using before, it certainly gives better results for less lag time! Would I be best off (unless I can get real sensor data) to implement some sensor fusion? I was thinking by combining accelerometer data with the baro, I can smooth it out, but I am not quite sure how to do that yet. Commented Jul 4, 2015 at 1:12
• I am starting to feel like this is going well past the scope of my original question. I have been doing some research and it may sound like to get good data I may need to implement a full IMU system. Do you have any reading suggestions for books or otherwise? I frankly don't know where to start. Commented Jul 4, 2015 at 1:51
• @ZNaught Sensor fusion will certainly give you better data, and this is where the Kalman filter excels. However, it can quickly become hairy because you'll automatically be using a 2-D filter. An IMU is probably the best sensor you can use for this application - some already do the filtering for you. If you're going to be using an accelerometer, you'll need to process its data with a gyroscope in order to retrieve accurate data. In regards to books, are you looking for references on filtering, control systems, both, or other? Commented Jul 4, 2015 at 2:06
• Also, I know you said your background is in aerospace engineering (degreed, student?), but what's your mathematical background? I'm not sure what math courses aerospace engineers take. Commented Jul 4, 2015 at 2:07
• The IMU I am currently using is the Adafruit 10DOF which does absolutely no filtering (except the sensors themselves which you can select resolutions and such). It has a gyro, compass, accelerometer, and barometer (with temp). As for books, I could use background on both of those subjects for sure. My math background is up to differential equations and I have a year left until I graduate with my BSE. I did find this awesome resource, it seems to be what I am trying to do: home.earthlink.net/~david.schultz/rnd/2004/KalmanApogeeII.pdf He has a bunch of info on his website too Commented Jul 4, 2015 at 2:23

You can use Excel to smooth out data and do graphs if you need to. That's what I use myself. Easiest way is to create a table and type formula to make average from range of numbers, smoother data needs a bigger range of numbers.