Kalman Observer

[brm]

hi,
i have been working on a kalman filter to get rid of the gyro and accelerometer noise.
the result is nice in comparison to the time spend on tuning.
the time spend for pid tuning is zero

http://youtu.be/tk-bv9rAdWY

i need testers for the next few steps.

Code: Select all

    // Radio
    parseRcChannels("AETR1234");
    cfg.deadband = 0;
    cfg.yawdeadband = 0;
    cfg.alt_hold_throttle_neutral = 20;
    cfg.spektrum_hires = 0;
    cfg.midrc = 1500;

this is the radio configuration for the attached hex file.

it is not my intention to destroy your flying vehicle.
i'd like to move on the flight quality.

[brm]

attached the picture of the settings screen.
the range of pid values is large.
in my case i can nearly double the p value - starting at 3.5

also new a filter for the bmp085.

[crazyal]

your changes look interesting. could you please also release your code changes ? that would be really nice

[brm]

your changes look interesting. could you please also release your code changes ? that would be really nice

no big issue.
there is a need to verify the filter on a larger quad.
just to see what happens
i hope tc will grant me svn access.
then you get it automatically.

the code changes are simple:
- no lpf filter defined
- no gyro smoothing
- no filters configured for the mems devices
- simple kalman filter for gyro and accelerometer
noise is tackled by the kalman filter.

the but is simple as well - you can overtune the pid loop

[ABL]

I tested this firmware today.
Hard to tell, if it's better or worse (too bad weather). At least, flies not worse than original baseflight ;-)
http://www.youtube.com/watch?v=nr_bVPg_4Xc

[brm]

thanks for testing!
in your video i think the 'smoothing' is somewhat too high.

i would be interested to know what board you are using - green, blue or white?
when you have the multi wii gui connected, what is shown as accelerometer values after
the calibrating action?

there is a mismatch between the gyro reading and pwm out output frequency.
i set the looptime to 3000 for the next few tests.
on my larger quad i get problems - the result is not what i expected but.
i hope to have an optimized version for large and smaller size quads soon.

at least you got snow!
we have 12 degree celsius ... too warm.

[ABL]

thanks for testing!
in your video i think the 'smoothing' is somewhat too high.

12m/s wind is something like 40km/h, i doubt you can tell if it's "over-smooth" when i'm non-stop correcting against wind

i would be interested to know what board you are using - green, blue or white?

written in video description - v4 (means white), but i have all public revisions available.

when you have the multi wii gui connected, what is shown as accelerometer values after
the calibrating action?

0
0
256

there is a mismatch between the gyro reading and pwm out output frequency.
i set the looptime to 3000 for the next few tests.

I'm using 5000 as looptime (mentioned in video description)

at least you got snow!
we have 12 degree celsius ... too warm.

Yeah.... And -18°C - no fun....

I didn't tested magnetometer, but baro (althold) works OK.

[brm]

hi ABL,
i just clicked to youtbue video to see it.
the explanations are well done - thanks.

in the mean time i have flown a few times in the garage.
hovering and altitude hold are ok.
i managed it to reduce the dampening until i saw some oscilations during acceleration.
the attached version should *not* show oscilations during aceleration.
but this depends on the quad ...

your quad looks very nice.
self made?

best robert

[brm]

your changes look interesting. could you please also release your code changes ? that would be really nice

hi,
i am off for the next few days - up in the mountains.
finding the right parameter set is time consuming.
after some more testing i will release it.

[ABL]

i managed it to reduce the dampening until i saw some oscilations during acceleration.
the attached version should *not* show oscilations during aceleration.
but this depends on the quad ...

I will test that tomorrow...
IMO biggest challenge would be "proper position hold" without GPS (resistance to wind, acceleration, same for gimbal mix).

your quad looks very nice.
self made?

Yes... Drawings available if needed, not so hard to cut with hand tools and very, very crash-resistant...

[sacarlson]

I would like to try the kalman filter for at least gyro pitch and roll in wing mode. I now get a lot of jitter in my elivon servos now even with gyro filter set active and P set as low as 0.5 . I don't think it's possible to try what you have without the source of your changes to enable modification of defines. I can setup another github.com to collaborate on this and/or other projects in multiwii if that would make it any easier for you to share the code. Otherwise I'll try some kalman code I see in other places in arduino here http://arduino.cc/forum/index.php/topic,52717.0.html. for more details of the aircraft and attempted and continued flight test see http://wings.surething.biz/?p=954

[ABL]

I would like to try the kalman filter for at least gyro pitch and roll in wing mode. I now get a lot of jitter in my elivon servos now even with gyro filter set active and P set as low as 0.5

In flight?
I think you're doing something wrong...
I tested FW setup (on baseflight mini board) many times, in gyro mode it works very well. This filter won't help here (or at least won't do noticeable improvement).
Post all settings, configuration file, airframe setup, video (in flight! Do not test on ground!) in correct forum section, i'm sure someone will come and help.

Otherwise I'll try some kalman code I see in other places in arduino.

...Are you sure that you're posting in correct forum section? Arduino?

[ABL]

Tested new hex file on old quad with ancient FF board.
It flies, but... P settings are too high, compared to baseflight (it was flying OK with roll/pitch P = 6, now it's oscillating a bit). And feels... Somehow "twitchy".
looptime=3500, everything else (what affects flight) left at default.
Need to re-test on another quad or another board, will try to do that tomorrow. Maybe others will join too...

[crazyal]

your changes look interesting. could you please also release your code changes ? that would be really nice

no big issue.
there is a need to verify the filter on a larger quad.
just to see what happens
i hope tc will grant me svn access.
then you get it automatically.

the code changes are simple:
- no lpf filter defined
- no gyro smoothing
- no filters configured for the mems devices
- simple kalman filter for gyro and accelerometer
noise is tackled by the kalman filter.

the but is simple as well - you can overtune the pid loop

you could just fork the svn mirror on github: https://github.com/multiwii/baseflight
git also makes it easier to keep your code up to date to the current svn version.
It'd be great if you publish your source there

[brm]

ok, back - just fighting with a stupid lumbago...
the fork has been created.
just fighting wit hstupid java exceptions within eclipse :-/

today i flew a bit more - the gyro filtering is not yet correct.
needs some testing...

i'd like to change more ... timecop prob. would say NO
over time it will change ... i am also playing with quaternions.

[ABL]

the fork has been created.

Where?
Btw i'm still flying previous version (posted in this thread) - works fine!

[brm]

on github - search for baseflight and pick up the user brm007

the gyro is some sort of problem ... :-/
the whole stuffe needs more flight time ... until i am really happy.

[brm]

the fork has been created.

Where?
Btw i'm still flying previous version (posted in this thread) - works fine!

hi ABL,
with previous you mean the first hex posted here?
i run into a small calibration issue when testing the first hex posted here.
the copter was not really level...

[brm]

Hi ABL,

I need your help.
Can you provide me with the drawing of the quad you use?
I will build a spare one ...

As a test I attached a 3300mah battery.
just to see what happens - this after a failed programmatic flip ...
Before landing upside down I flew through heavy wind.
I did not loose control over the quad.
Looks like my approach is safe.

http://youtu.be/HjitL2cyfa4

[ABL]

Can you provide me with the drawing of the quad you use?

No problem!
Drawing here, without motormounts. Motormount drawing: simple and advanced (or simply screw to arms without any mounts, if using alu tubes instead of wood).
Solidworks drawings, if needed (3D could be viewed using free eDrawings viewer, but for FEA calc and other features you still need to use solidworks. FEA data not included) are here.
That's slightly modified (incl. gopro mounts - several designs, etc) spiderquad model, compared to original (not mine), found here.
Hand-cut (no CNC) in few hours... And do not make "bottom deck" if you do not plan to attach second camera - quad will be more resistant to wind.

As a test I attached a 3300mah battery.
just to see what happens - this after a failed programmatic flip ...

Great!
I will check latest version when weather permits (snow, rain and other <censored> here, no way to fly... Wait, maybe i should make waterproof quad =)

[brm]

ok, I did run into shh2 issues.
I attached the modified sources - this is just a dropin.
I noticed in sensors.c is a change for the mag orientation.
In the latest svn code this is commented out - do the same if you run into a mag problem.

For me the pre filtering of the mems devices was fun.
It clearly shows an enhanced stability

Now I am ready for FPV - the base is stable.

[ABL]

AFAIK you should have write access to afrodevices@googlecode. Why not put code here?
Also, could you add compiled hex?
Thanks

[Crashpilot1000]

@BRM: Many thanks for sharing your code here! Can you have a look at the precision improved main PID controller of my version viewtopic.php?f=23&t=1947&start=120#p28812 i think combinig both will be overkill.

Cheers
Kraut Rob

[crazyal]

ok, I did run into shh2 issues.
I attached the modified sources - this is just a dropin.
I noticed in sensors.c is a change for the mag orientation.
In the latest svn code this is commented out - do the same if you run into a mag problem.

For me the pre filtering of the mems devices was fun.
It clearly shows an enhanced stability

Now I am ready for FPV - the base is stable.

thanks for sharing your code. If I am not mistaken the implementation of the kalman filter you used is no more than a lowpass filter, as described in this article http://interactive-matter.eu/blog/2009/ ... an-filter/. Somewhere in the comments even the author says it's just a low-pass-filter.

[brm]

no, it is a kalman filter - have a look at the wikipedia artical.
you can further reduce it ot make a lp filter out if it.

the main reason doing so is that the kalman filter gets the noise out of the signal.
with a lp filter you can severly confuse the sensor fusion.
especially mahony type filters simply cause a copter to crash with brute force lf filtering.

for me this work is a proof of concept.
lp filtering on mems devices is not the way to go.
you need a smarter aproach

[brm]

@BRM: Many thanks for sharing your code here! Can you have a look at the precision improved main PID controller of my version viewtopic.php?f=23&t=1947&start=120#p28812 i think combinig both will be overkill.

Cheers
Kraut Rob

no - a combination of both is ok.
the p and i term will not change that much on fast decends but the d-term will act on changes.

the mw stuff needs a propper re-write to use floats and 32 bit arithmetic all over the place.
more work to be done ...

[ABL]

Tried latest version... Too bad, weather does not permit good testing, but it flies
Notes:
- mag is not detected at all (not listed in 'status' output)
- there's no support for gimbal mixing (introduced in R239) - could you add this to code too?

Thanks!

[brm]

hmm,
I did notice a few probs with the mag - do not remember that I disabled the mag.
Need to do some research.
I have access to svn - will create a seperate branch based on the latest code.
will take a few days ... need to do some other tasks

Thanks again for the drawings - lots of timeslices needed for cutting.
should I do this first ?

[ABL]

Thanks again for the drawings - lots of timeslices needed for cutting.
should I do this first ?

No, go code - if you want, i could send you plywood spiderquad kit (i do not have time to finish all types of copters i want):

2x lighter than fiberglass version, but, ofc, not so strong - so won't hold crashes so well, compared to fiberglass version....
If that would help you to code faster - just PM me your address, will ship asap (ofc, for free - for your work with code), so you will need only poplar or basswood legs, mount electronics and go flying...

[brm]

ABL,
I take your offer.
The mail channel seems to have be set to stopped.
My box shows one outgoing email since an hour...
If you do not see anything you can ping me on: brm@bluemail.ch

thanks!

[SVentas]

no, it is a kalman filter - have a look at the wikipedia artical.
you can further reduce it ot make a lp filter out if it.

Hey brm,
I did some investigations and modelling. Unfortunatelly I must admit, that this Kalman filter acts EXACTLY like first order low pass filter.


In this line:

Code: Select all

state->x = state->x + k * (measurement - state->x);

k is a constant value (see graph for k), because k depends on p, and p depends on q and r which are constants.
So there is no defference in output between single dimention Kalman filter You use and first order low pass filter (see graph for "Kalman" and "FO_LPF"), except Kalman filter takes more time to calculate.
I think we need to use more efficient n-th order IIR filters like Butterworth, Chebyshev or Elliptic... What do You think?

[brm]

hi,
it is a simplifed kalman filter - i agree.
and i have a full kalman filterin the back - alas matlab code translated to 'c'.
will use more time because of the matrices.

you need to create a bode diagram - then you might 'see' more.
using madgwick i did run into some crashes using a combination
of lp filtering in the pid generator and a Chebyshev filter for motor vibrations.

but the result is amazing.

my noise strategie is simple - i read the mems device at 1.2 khz downsample it to 300hz and feed at 300hz the simplified kalman filter.
works for me.

[SVentas]

and i have a full kalman filterin the back

At first I tought You implemented Kalman filter in IMU .
Do You think it is worth to try Kalman from picopter project to fuse acc and gyro?

my noise strategie is simple - i read the mems device at 1.2 khz downsample it to 300hz and feed at 300hz the simplified kalman filter.
works for me.

Do You read MPU6050 via I2C (@ 400kHz speed) at 1.2kHz or do You use FIFO watermark to indicate collection of 4 samples and than grab all the samples at once?
I think that the better strategy is to feed simplified Kalman filter and then downsample, because then You will not get aliasing problems (at least it's the way decimation/downsampling works).

[brm]

typedef struct
{
float m_x[2];
float m_p[4];
float m_q[4];
float m_r;
} kalman1D_t;
is in place - works - not yet flown.
give picopter a chance - i have flown it a few times.
my firstorder simplified kf and the pico filter behind.
to me it needs more work.

running some sort of filter at 1.2 khz needs a more powerfull cpu i think.


there is an ekf for this type of filter.

[brm]

the new full flegded kalman filter is in place.
now it also covers the derivative change

i have flown the current build in my garage.
it does it well behaved.

you can watch the cycle time
not sooo much cpu cycles used

added:
the reactions on my larger on stick movements are to nervous.
i made the behaviour somewhat more lethargic.
for larger quads you need the relax version.

[brm]

http://adrianboeing.blogspot.ch/2010/05/kalman-filters.html
there is another interesting filter which looks worth to test.

the derivative term grows wy to fast - way to nervous reactions as a result.
this is true for the upper compiled firmware.

will do more testing tomorrow.

[ABL]

PPM not supported in hex files above?

EDIT: tested on another board, looks like everything is OK.
Going to test this on another quad, brb

[timecop]

Huh, *removing* ppm support would be more trouble than its worth.

[brm]

hi all,
not to mess up wit the default settings i have been creating things like:

Code: Select all

#ifdef ROBERT
    // futaba
    cfg.midrc = 1520;
    cfg.mincheck = 1120;
    cfg.maxcheck = 1900;

    featureSet(FEATURE_PPM);
#endif

before i have been using:

Code: Select all

    pwm_params.usePPM = true; //  feature(FEATURE_PPM);

in essence wrong wording from my side.
if you use cli to set options then everything is ok.

it is amazing - i see something blue above me.
good chances to see the sun.

added:
i flew a few times with my larger quad.
when rising the looptime it is more relaxed flying.
i use standard non-re-flashed esc's

also interesting: http://en.wikipedia.org/wiki/Alpha_beta_filter

[brm]

http://youtu.be/-2o43c6was0
This is the alpha beta filter in action.
The frame is too soft and produces enough vibrations when accelerating faster.

To me the testing made clear there is a need for a defined lag.
having the derivative change as correction leads to hypernervous copter
thus there is a need to add a defined delay ... a pt1 element in before filtering the gyro values?!?
another filter named here before may do the job as well.

[brm]

today i tested my fpv stuff - just the video link and camera.
and i stick with the pt1 element in front of the simple kalman filter.
both quads do fly nice - real smooth to carry a camera.
and better as in the prev. video.

[ABL]

Heh, i returned after testing this version with 'relax' suffix, and noticed many other versions around...
Did some testing - IMO very good in gyro-only mode, something wrong in acc+gyro mode (oscillations, maybe lowering P will help).
Crash - dunno what caused that, few versions:

• i accidentally hit mag (headfree) activation switch and turned it ON. MAG isn't calibrated after reset....
• i lost orientation (strange a bit, copter wasn't so far)
• some bug? Doubt so

Anyway, quad will be fixed in 2-3 evenings and will fly again. Gopro black received few scratches but will live :F
Link to video: http://youtu.be/C-6tahlsFQE

My settings:

Code: Select all

vbatscale = 112
looptime = 3500
p_pitch = 68
i_pitch = 18
d_pitch = 25
p_roll = 68
i_roll = 18
d_roll = 25
p_yaw = 80
i_yaw = 56
d_yaw = 0
p_alt = 18
i_alt = 11
d_alt = 7
p_level = 58
i_level = 10
d_level = 20

Will test other versions later.
At least this version is a bit worse than earlier "kalman-LPF' version.
Good: good yaw lock (original baseflight still rotates on fast descents), good gyro mode
Bad: oscillations in acc mode (as i told before, maybe lowering P will help, with original baseflight i can fly with P=7.6 without wobbles), a bit strange reaction to pitch/roll in acc mode (can't describe what's wrong yet)

[brm]

hi ABL,
thanks for testing - and you have a very nice environment.
here it is way too hot - but for today at least very sunny.

in essence there are now 2 additional filters.
just to see what is appropriate and what is wrong.
a certain delay is ok and filters out the bad stuff.

i was a little but puzzled - but after trying the alpha-beta filter it turned out
that i need to get rid of the bad frequencies above the the dynamics of the quad.
this is the reason to add pt1 element before the kalman filter.
both copters do fly well behaved.

the wobble in acc mode is prob due the phase difference between gyro and acc filtering.
this has been removed with the pt1 element which allowed me to reduce the delay in both filters.

now i can play with the pt1 element.
expect more to come

when it wobbles on faster descends then increase the d-term for the pid's roll and pitch.

best robert

[brm]

to dark to get a video - a gopro is a nogo when the light is bad.

i am happy on my smaller quad - i upped the cuttoff frequency of the pt1 element to 200hz.
and my small quad flies like a champion.

[ABL]

Thanks, i will check tomorrow if possible (need some repairs on all of my copters, so...). Will write feedback here.
Probably i will take laptop with me, to try changing PID's if i expierence oscillations like today.

[brm]

hi ABL,
can you provide me with the dimensions of your quad?
thanks in advance.

[ABL]

hi ABL,
can you provide me with the dimensions of your quad?
thanks in advance.

I provided links to drawings in previous post - download free edrawings viewer if you need 3D views/measurements - that will be easy ;-)

[brm]

http://youtu.be/tv73kWezRwc
the result adding the pt1 element.
ready for foto shooting

at least a good base for my fpv trials.

[nicog]

processing my brm....

[brm]

yes ... processing ... 58 percent.
google is slow today.