Kalman Filter for height and acceleration - height

Question

im working on a stm32f417ve arm processor and trying to implement a kalman filter for fusing accelerometer data with height (pressure sensor) data.
I want to know the estimated vertical velocity and position. The accelerometer readings are rotated from body frame to earth frame, thats not the problem.
I've already searched a lot on the internet and also found some interesting things, but I'm not sure if my situation fits into the other ones i've found, so I'm here :)
This post ( Using Kalman filter with acceleration and position inputs ) is very similar to this one, but i need a little bit more help.
I've also got an MPU6000 as 6DOF imu and a MS5611 baro. I think, the best way to combine these data is to use the acceleration as a control input, am I right?
Maybe someone could look at my matrices and formulas to tell me, if its right or not.
Formulas:
//PREDICT
x = A*x + B*u
p = A*p*AT + Q
//UPDATE
Innovation = (H*p*HT + R)^-1
K = p*HT*Innovation
x = x + K*(y-H*x)
p = (I-K*H)*p
Matrizes:
#define NumState 3
#define NumInput 1
#define NumOutput 1
static float32_t xAr[NumState][1];
static float32_t uAr[NumInput][1];
static float32_t yAr[NumOutput][1];
static float32_t AAr[NumState][NumState];
static float32_t BAr[NumState][NumInput];
static float32_t HAr[NumOutput][NumState];
static float32_t QAr[NumState][NumState];
static float32_t RAr[NumOutput][NumOutput];
static float32_t PAr[NumState][NumState];
static float32_t kAr[NumState][NumOutput];
static float32_t IAr[NumState][NumState];
I put the acceleration into vector u and height into y.
The Matrix IAr is just a identity matrix, so its diagonal elements are 1.
RAr[0][0] = 0.1f;
QAr[0][0] = 1.0f;
QAr[0][1] = 1.0f;
QAr[0][2] = 0.0f;
QAr[1][0] = 1.0f;
QAr[1][1] = 1.0f;
QAr[1][2] = 0.0f;
QAr[2][0] = 0.0f;
QAr[2][1] = 0.0f;
QAr[2][2] = 0.0f;
uAr[0][0] = AccZEarth;
yAr[0][0] = Height;
HAr[0][0] = 1.0f;
HAr[0][1] = 0.0f;
HAr[0][2] = 0.0f;
BAr[0][0] = (dt*dt)/2;
BAr[1][0] = dt;
BAr[2][0] = 0.0f;
AAr[0][0] = 1.0f;
AAr[0][1] = dt;
AAr[0][2] = 0.0f - ((dt*dt)/2.0f);
AAr[1][0] = 0.0f;
AAr[1][1] = 1.0f;
AAr[1][2] = 0.0f - dt;
AAr[2][0] = 0.0f;
AAr[2][1] = 0.0f;
AAr[2][2] = 1.0f;
IAr[0][0] = 1.0f;
IAr[0][1] = 0.0f;
IAr[0][2] = 0.0f;
IAr[1][0] = 0.0f;
IAr[1][1] = 1.0f;
IAr[1][2] = 0.0f;
IAr[2][0] = 0.0f;
IAr[2][1] = 0.0f;
IAr[2][2] = 1.0f;
PAr[0][0] = 100.0f;
PAr[0][1] = 0.0f;
PAr[0][2] = 0.0f;
PAr[1][0] = 0.0f;
PAr[1][1] = 100.0f;
PAr[1][2] = 0.0f;
PAr[2][0] = 0.0f;
PAr[2][1] = 0.0f;
PAr[2][2] = 100.0f;
It would be really great if some of you guys could take a look and tell me wheter im right or wrong!
Thanks,
Chris

Answer 1

The first thing to determine is whether the two sensors you intend to use together are a good complement. The MEMS IMU position will diverge quickly as the double integration errors pile up. To successfully use it in this application at all you will have to calibrate its bias and scale. Those will be different on each axis, which, given your one-dimensional state, will have to be applied outside the filter. Since you are probably going to be outdoors (where an altimeter is interesting) your bias/scale calibration should also be temperature compensated.
You can easily test the IMU by doing the x = A*x + B*u loop while the IMU sits on your desk to see how quickly x[0] becomes large. Given what I know about IMUs and altimeters (not as much as IMUs) I would guess that your IMU-derived position will be worse than your raw altimeter reading within a few seconds. Much faster if the bias and scale aren't properly calibrated. The Kalman Filter is only worthwhile to "fuse" these two sensors if you can reach a point where the short-term accuracy of the IMU is significantly better than the short-term accuracy of the altimeter.
If you do proceed with the KF, your structure looks generally good. Here are some specific comments:
You model acceleration as -x[2]. (The minus sign is due to your matrix A. I'm not sure why you chose to negate acceleration.) I don't think having acceleration in your state is doing you much good. One of the advantages of the ... + B*u method of using the IMU is that you don't have to retain acceleration (as your B matrix demonstrates). If acceleration is a measurement you have to have it in your state vector because H=[0 0 1].
You have not made any effort to choose P, Q or R. Those are the most important matrices in the KF. Another answer here might help: https://electronics.stackexchange.com/questions/86102/kalman-filter-on-linear-acceleration-for-distance/134975#134975

Answer 2

thanks for your answer!
Until now, I'm using a complementary filter to fuse the acc data with the baro data. All three axis of the acc are already compensated.
Right now, I've got a 1D-kalman filter which reduces noise of the baro output while keeping the phase delay quite smale, thats the reason why I don't use a lowpass filter.
I'm calculating the derivative of the baro data to get velocity based on the baro, which has about 100ms delay.
This velocity is then feed into the first complementary filter together with the acc calculated velocity by integrating it.
The second complementary filter uses this fused velocity (which is drift-free & has nearly no delay) and integrates it to fuse it with the baro altitude data.
This works quite well, but I wanted to try the kalman filter to see, wheter it's possible to get a more accurate data out of it.
In the internet, if found this paper: http://www.actawm.pb.edu.pl/volume/vol8no2/06_2014_004_ROMANIUK_GOSIEWSKI.pdf
It seems to match my "problem" very good, so I decided to use it as a starting point.
The negative sign in my matrix A comes from this, maybe due to their mounting direction. I'm gonna check that ;)
Best Regards
Chris