height


Kalman Filter for height and acceleration


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
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
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

Related Links

Falling Object Initial Value
Subliem text3 AutoFileName: How to change it, that it will add width and heigth attributes in Sublieme text 3
Finding the Table height and Width in Word Document
FontForge how to get height of contour in script
how to set another elemtnt's height based on slider height in this context
Classic asp control character height in dropdown
Auto adjust row height to max height value
How to calculate Height of any control which is loaded in DataTemplate?
Xamarin Forms - Get the size of an AbsoluteLayout
Amcharts : small (sometimes 0px) chart height on page load (in default tab)
Spacing between 2 lines HTML/ PHP and CSS
How to change embedly height
Cell/Row heightand vertical align in PhpWord
How to make 2 divs to fill 100% viewport height
Script to remove element while clientHeight < “size”
how to auto-adjust App.js textarea height to fill the remainder of screen?

Categories

HOME
app-inventor
payment-gateway
redis
rotation
textwatcher
ubuntu-16.04
angular2-directives
user
browserify
nixos
cpanel
local
activesync
line-api
sendkeys
bootstrap-popover
cairo
chocolatey
styles
google-maps-android-api-2
url.action
tarantool
multiplayer
azure-servicebus-queues
searchbar
activepython
lync-2013
image-quality
upnp
reply
gzip
piecewise
protobuf-net
expand
ipfw
outsystems
modelandview
reactivemongo
nunit-3.0
vmd
readfile
ntfs-mft
android-maps-v2
g1gc
crop
worker
asmx
python-idle
rapidweaver
number-theory
amazon-machine-learning
persistent
search-box
unity3d-editor
visual-studio-templates
festival
iostat
angularjs-ng-pattern
foxit
spark-cassandra-connector
group-concat
qt-linguist
loopj
spservices
getrusage
windowlistener
type-mismatch
divide-by-zero
cpu-cores
hiera
custom-url
orientation-changes
inputaccessoryview
gyroscope-framework
and-operator
ember-components
suffix-array
eclipse-classpath
ssms-addin
openkinect
confusion-matrix
windows-mobile-6
cdata
osx-snow-leopard
robospice
driver-signing
sql-server-2012-web
image-zoom
va-list
pascals-triangle
objcmongodb
transitive-closure-table
marmalade-edk
sequelpro
enumerators
css-tables
entity-framework-ctp5
v4l
querystringparameter
throttling
smooth
svn-hooks
revision
nt4
system-codedom-compiler

Resources

Mobile Apps Dev
Database Users
javascript
java
csharp
php
android
MS Developer
developer works
python
ios
c
html
jquery
RDBMS discuss
Cloud Virtualization
Database Dev&Adm
javascript
java
csharp
php
python
android
jquery
ruby
ios
html
Mobile App
Mobile App
Mobile App