Kalman Observer

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

[ABL]

Done, tested latest version, posted here.
I changed some variables - increased roll/pitch D from 25 to 30, lowered P on pitch/roll from 68 to 64.
Flew without oscillations in normal flight (still oscillates when descending, will increase D even more).
Too bad, gopro corrupted my last video, was really fun to do takeoff and landing directly from car, etc ;-)
Tested gyro, acc, baro, mag - everything looks OK.
Minor problems:
- slight drift in acc mode, can't compensate with acc trim (drifts to one side, one click away - drifts to another side). Not big and should be fixed with "pos hold with acc" algo
- still wobbles when descending in propwash - will try to increase D more

I could say - flies really well, i even tried to climb approx 100m, where wind is strong - no problems:


...another test for NAZE32 hardware is incoming - today is ~ -12°C, but soon it will be ~ -25°C - will test how hardware will handle low temps =)

[nicog]

brm, you use it in acc mode or gyro mode?

I will do some testing this week, I had tried the pt1 element some time ago (coded myself). I will try yours. tell you how it feels

[brm]

i started with gyro only and switched over to acc mode.

[brm]

http://youtu.be/o-ulU1dO_L8
the gyro filter settings for fj450 frame in the video are not yet correct.
with no changes on the pid settings.
a new version has been committed to svn.

[brm]

work continous ... for the integrator i need the filtered accels in the ned frame.
any acc_z correction should also be applied with the value from the ned frame.
http://youtu.be/j590bEWvsX4
the baro is not covered.

[brm]

http://youtu.be/RYHVJ6jq5rg
until a few days ago quite a few where thinking that a low settings of the dlp filter was 'the' solution to remove noise from the accelerometer.
in the end i had a version flyable for my small quad and not for my larger quad and vice versa.

going the opposite way seems to be solution.
the dynamics of a quad is above the 150 hz. setting the dlp frequency to 42hz for the mpu-6050 creates some rare effects.

sources will be updated later on.

[ABL]

I tested latest binary from google code (robertb branch aka brm), looks OK!
No wobbles on descent (well, small ones), holds well.
Bit drifting to sides, but... All versions do the same, pos hold on acc's not implemented. Used - gyro, acc, baro, mag not tested (booooring to re-calibrate it each time i flash new FW :P )

My settings:

Code: Select all

map taer1234
feature ppm
feature vbat
feature motor_stop
feature servo_tilt
set gimbal_flags=16
set gimbal_pitch_gain = 25
set gimbal_roll_gain = -26
set gimbal_pitch_mid = 1350
set gimbal_roll_mid = 1650
set vbatscale = 112
set looptime=3500
set p_pitch = 64
set i_pitch = 18
set d_pitch = 30
set p_roll = 64
set i_roll = 18
set d_roll = 30
set p_yaw = 80
set i_yaw = 56
set d_yaw = 0
set p_alt = 18
set i_alt = 11
set d_alt = 7
set p_level = 58
set i_level = 10
set d_level = 20

Go and try! =)