pt1 element

[brm]

i have to admit that in the past i have always flown with a pt1 element (pid calculation).
so i do the same here.
it helps on fast descends and nearly eliminates the wobble.
for fpv flyers a must.
in the file "mw.c" we simply add:

// July 2012 V2.1robert

#define F_CUT 20.0f
#define RC 0.5f / (M_PI * F_CUT)

in the loop() function we add:
static float lastDTerm[3] = { 0.0, 0.0, 0.0};

and after the comment we add the line:

Code: Select all

        // **** PITCH & ROLL & YAW PID ****    
        float dT = cycleTime * 1e-6;

nearly at the end we find the location where the dterm will be calculated - here add:

Code: Select all

            // calculate PT1 element on deltaSum
            deltaSum = lastDTerm[axis] + (dT / (RC + dT)) * (deltaSum - lastDTerm[axis]);
            lastDTerm[axis] = deltaSum;
            DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; // 32 bits is needed for calculation

the last line is the existing code.
happy flying.

[nicog]

Hello brm,

I have a couple of questions.

Your define need some tune? I'm adding this to baseflight, so doing these 2 values tunable are easy as cookies.

Thanks.

[nicog]

Nevermind, I added Fcut to cli. Now, a test drive. tell results later.

[brm]

Nevermind, I added Fcut to cli. Now, a test drive. tell results later.

hi,
easy - i didn't tune my small jakub frame.
the p value has been set to 3.5 before and after the mod.
nothing else.

due to the fact that it is a small low pass filter and is working on sum of the d-term deltas it is no dramatic change.
as i said - the best benefit is prob. comming from flying in level mode.

lately i think a pt1 element is a must in every pid controler.
in hour office it is considered as 'standard'.

starting to work on a pidz controller

best brm

[nicog]

Ok test flew it. Not big diference with or without, so I'm leaving it in. Time to pass the patch to Timecop.

thanks.

I left the fcut value setteable from cli.

[Crashpilot1000]

@BRM:

Hi!
I tested your pt1 element and i think it really improved descent stability. Due to my limited understanding of these things, i wonder what the PI does in that context but it works .
Since deltaSum is only 16 Bit i think it may improve your approach to do it as float to store it more precise for the next run?

Code: Select all

float DeltaSumPt1 = deltaSum;             // goto float for precision
DeltaSumPt1 = lastDTerm[axis] + (dT / (RC + dT)) * (DeltaSumPt1 - lastDTerm[axis]); // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624
lastDTerm[axis] = DeltaSumPt1;                  // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624
deltaSum = DeltaSumPt1;

Thank you very much for doing some "dt" on the mwii pid controller, wich is missing in original mwii!

Cheers

Kraut Rob

[brm]

hi rob,
the derivative term is quite dangerous.
the main goal is making it more predictable - therefore the filtering.
i haven't tested the float variant.

for shure i am not flying without pt1 filter

[Crashpilot1000]

I took the float version for a spin and it worked well. I am not sure if it is an improvement over your original version but it works. BTW i put it in the harakiri03b (with float) .
Currently i am experimenting on putting the whole mwii pid controller to float and dt stuff. The arducopter pid library seems to be a good starting point (http://code.google.com/p/ardupilot-mega ... s%2FAC_PID)

Greetings
Kraut Rob

[Crashpilot1000]

Hi!
After nearly a year this thread needs a refresh ..
I am currently reworking the GPS D-part (once again..) while doing that I was wondering if your extension for the maincontroller D part would make sense before the mwii moving average filter. What I mean is, maybe the frequency filter may work better if applied before the moving average, so the moving average gets prefiltered data.
The Code from actual harakiri mainpidcontroller would look like this then:

Code: Select all

...
            PTerm          -= (gyroData[axis] * dynP8[axis] * 0.0125f);
            delta           = (gyroData[axis] - lastGyro[axis]) / MwiiTimescale;
            lastGyro[axis]  = gyroData[axis];

// Change start

            tmp0flt           = delta1[axis] + (dT / (MainDpt1Cut + dT)) * (delta - delta1[axis]);
            deltaSum        = tmp0flt + delta1[axis] + delta2[axis];
            delta2[axis]    = delta1[axis];
            delta1[axis]    = tmp0flt;

// Change end

            DTerm            = deltaSum * dynD8[axis] * 0.03125f;
            axisPID[axis]   = PTerm + ITerm - DTerm;
...

I could do a quick spin with that code (between rain) with hefty wind. I am not sure if it's really better but I could see a difference when altering the frequency (tested: mainpt1cut = 15 and 20. 20Hz is better), that wasn't obvious with the previous method.
On the other hand the altered D calculation (first freq fiter, than moving average) in the GPS area seems to be better (also just a short test possible - what a sh** weather).
I think it's worth to investigate that further.

Cheers
Rob

[brm]

hi,
i would not add anything called time scaling.
as usual there is jitter on the time axe.
the pt1 calculation takes this into account.

the pt1 element should be calculated using the delta from round to round.
if there is a need to average this new delta calmed by a pt1 element, then this is an indication noise/vibrations.

i am flying something inbetween - 17 hz as cutoff frequency.
have not had to much time flying here - too cold
best
robert

addendum:
cascading two filters is not a very good idea. here it consumes cpu cycles and memory.
the output channels should be fast with no delay - with every filter being added you increase the delay.
i managed it to successfully crash a copter when creating to much delay for the d-term.

[Crashpilot1000]

I def. see your point. Adding extra filters will produce phaseshift/lag of course and will - if too much - drive havoc on the controlled system.
I have to apologize that I got your first post (jan) wrong, because I thougt you added the filter at the end of the mwii D calculation. The mwii D calculation however does an moving average (3 elements, division is done together in the "DTerm" line) and adding your Hz filter was working so fine....
Well I will delete the mwii average and give it a shot with just your Hz filer on...
Another thing I noticed during fiddeling (and think is worth mentioning):
Since harakiri is doing that pid stuff in floats - than puts the result into the int16_t axisPID (like mwii does) - these values are than processed in the mixer, wich is based on float expressions. So the idea was to pass the axisPID in float and do the mixing in float as well - so just the last step (pwm value) is in int again. I was afraid that this more precise procedure could produce some jittering in flight (that would have been filtered by formatconversions otherwise) but it woked really well and is a nice icing on the cake concerning the flightbehaviour. The most noticable improvement is the yaw axis in flight - and after thinking about it - it is clear since the mixer is working with more precision then and the momentum distribution for yaw is critical (on copters with even numbers of rotors...). (For obvious reason the changes are minimal but in any case: the temporary repo "SGTodaysSnapshotMavlinkWPrwTest5" has that code).

EDIT 2100: I tried the "pt1" without the moving average and ended up with a shacky locking copter (tried 10 & 20Hz). My tescopter has some vibrations going on, of course.... In the next step I tried to understand what a pt1 does by reading the wiki... well ... &%$# too dumb for that BUT I stumbled upon the pt2 part, googled how to program it and found out that it is actually a cascaded pt1 part. So I did that - and bingo! That is serious dampening (no moving avg).
What the copter does in the air when releasing the sticks in angle mode is something like shown in the pic here http://www.mertech.de/rt/images/image421.gif. Only having a nebulous understanding of what those pt1 and pt2 elements are, I would say pt2 is something to check out.
Hopefully that pt2 stuff helps the GPS D part as well. To be continued....

Cheers
Rob

[brm]

hi,
no problem at all ... i frequently read only a third of an email

http://de.wikipedia.org/wiki/PT2-Glied
higher order filters have the problem being mathematically unstable on sharp changes.
this pictures in the link do show the over shoot scenario.

the question of using a higher order filter is simple: do i allow the input signal to be modified.
for me a simple NO - thus no higher order filter.
also http://www.kramann.info/64_Regelungssys ... /index.php

best
robert

[Crashpilot1000]

Thank you very much for the kramann.info link!

I took the pt2 element changed pid controllers (both) for a spin and added +1 to the roll & pitch P and +2 on the Level P and opening Level D to 100 (for "non alex.khoroshko" controller - note: level D is not a real D) - knowing these settings would cause severe oscillations and the levelmode would almost kill the copter.
The test result was revolting: The copter was very snappy and you could see it wanted to oscillate (while pushing it hard) but was absolutely blocked from doing so !! Wow! Who needs autopidtuning when you have pt2 element at play that forgives too sharp settings....
While not seeing through your (undoubtly well founded) theoretical concerns, I can say practically it rocks the house - still have to find the practical downside of pt2 element.
I added the changes concerning Harakiri here as a zip file - that's better than doing too long code posts..

Cheers
Rob

[brm]

hi,
pls. update your git repo
would be very helpfull.

will give it a try.
maybe the d-term is less errorprone to higher order filters.
best
robert

[Crashpilot1000]

Upped with new folder..... don't hit me....

[brm]

i won't hit you as i cannot do it remote
http://www.rn-wissen.de/index.php/Regelungstechnik
to me you created a cascaded pt1 element at 20 hz.
not what i would call a pt2 element.
but i will give it a try ...

EDIT:
... i did.
the d-term causes the copter to oscilate.
so pls. reduce - no fun to fly

i changed a few things to test:

[Crashpilot1000]

Hi! BRM!
Oscillate? Damn! Did you fly the posted hex or something else? Because that cascaded pt1 seems to work like the holy grail on my setup.
I tested out your hexfile as well and loaded setup defaults. Next to the cycletime of 2ms I noticed that my copter always does acro and no self leveling that's very strange, since I handtested it and there it self leveled. It also flew without oscillations and that is a good thing with those PIDs, but I didn't try out more pid variations in the dark cold...
BTW The green LED flashed a lot like the gyro calibration would have a hard time so I relaxed that with "set gy_stdev = 15" something doesn't seem rigth whith that green flashing...maybe small angles? .. Hmm...
Cheers
Rob

[brm]

hi,
yes, i flew the original sw - build from the github sources.
then i reduced the d-term to 60 - flew great.

i am looking to reduce the various complementary filters to more 'direct' values.
the d-term is 'size' dependent.
what is the size of the copter you are flying?

best robert

[brm]

i am somewhat in doubt after an aggressive flying session.
on my smaller quad i got two 'breakouts'.
it might be possible that you are operating on the border of the system stability.

i made the fir filter optional.
will do the same to choose between the normal pt1 element and the cascaded version.
we will have guests ... it may take a while ...
edit: will change the pid calculation - as i am not a fan of doing so ...

[Crashpilot1000]

Hi!
Good idea to do some boxes for testing hexfiles! Changed Level D back to 50 to be on the safe side.
BTW my setup: DJI450 Frame (could not resist got it dirt cheap), Robbe 980Kv Motors (2827-26), Cheap carbon APC style props 10'' x 4,5 (or 4,7?) Lipo 3s 3300mAh, Skywalker 20A flashed with BLheli, all ca 1Kg. I have to setup my little quad for testing again (gaui 330 style).
Meanwhile reworked PH a little for windy conditions, the doublefilter (or whatever it is) does a good job on the gps D part (code is in repo).
Cheers
Rob

[brm]

hi,
the cascaded pt1 has the advantage not creating a 'swing' in the response.
it is slower as the pt1.
played monopoly on sunday afternoon with my girls - no time to continue.
gps is navigation - no sharp changes exptected - can be a solution here.
but still, take a pt1 and play with the cutoff frequency.
i have a pt2 element in mind - so we play a bit more ... for the navigation - not the sensor fusion.

Code: Select all

   // overwrite the default settings
cfg.acc_lpf           = 4;         // changed 27.11.2012 to 100
cfg.gy_cmpf         = 50;       // (10-1000) 400 default. Now 1000. The higher, the more weight gets the gyro and the lower is the correction with Acc data.
cfg.gy_cmpfm      = 100;       // (10-2000) 200 default for 10Hz. Now 1000 for 70Hz seems ok. Gyro/Magnetometer Complement. Greater Value means more filter on mag/delay

i also reduced the settings of the complementary filters as shown above.
flies well - the fir filter does a good job here.

[Crashpilot1000]

I see, do you have a hex to play with - or some piece of source to look at?

[brm]

will push it later on.

have a quanton right next to me.
time to play with the fpu

[Crashpilot1000]

Oh, yes the quanton is a lightyear ahead of naze hardware....

[Crashpilot1000]

@BRM: Thank you very much for commiting the code! Haven't had time to test it yet... still wonder how I am supposed to tune those kalmanfilter terms ...

Edit: While trying to understand it I scrolled through your referenced site: http://interactive-matter.eu/blog/2009/ ... an-filter/

The author states in the comments (26.4.2010) "Ouch ! I have overseen it.... " that it is actually a LPF and no Kalman.
So don't get me wrong but maybe your fir_filter.c might boil down to a low pass filter?

EDIT EDIT:
I did some experiments with your filter applying it to noisy BMP085 data.
Using:
cfg.q = 0.0625f;
cfg.r = 1.0f;
cfg.p = 0.22f;
Played around with the values but could only flatten + delay the resulting curve.
However the graph is exacly like a normal lowpass filter: lastbaro = lastbaro + 0.22f * (BaroAlt - lastbaro);
Maybe I did something wrong.
That is the core code I used:

Code: Select all

typedef struct
{
    float q;   // process noise covariance
    float r;   // measurement noise covariance
    float x;   // value
    float p;   // estimation error covariance
} kalman_state;

static kalman_state BaroKF;


float BaroKalmanFilterStep(void)
{
    static bool init = false;
    float k;
    if (!init)
    {
        init = true;
        BaroKF.q = cfg.q;
        BaroKF.r = cfg.r;
        BaroKF.p = cfg.p;
        BaroKF.x = BaroAlt;
    }
    else
    {
        BaroKF.p = BaroKF.p + BaroKF.q;
        k = BaroKF.p / (BaroKF.p + BaroKF.r);
        BaroKF.x = BaroKF.x + k * (BaroAlt - BaroKF.x);
        BaroKF.p = (1 - k) * BaroKF.p;
    }
    return BaroKF.x;
}

I also applied the "double Hz filter" set to 1.8Hz (Note: New barodata @29Hz) and the result was smoother and in phase with the lpf version.

Cheers
Rob

[brm]

if you replace the floats with a matrix, then you have a full kalman filter.
the simplification is based on reducing the matrix operations.

i use a simple pt1 element for the baro.
did you measure the noise?
if not then you need to do so.
what is a "double Hz filter"
if it is a cascaed pt1 then you have a 180 degree phase response.
with a pt1 it is 90 degree phase response.
pls. explain what is 'in phase' for you?

[Crashpilot1000]

Hi BRM!

I did some tests with different filtering by staring at the curves (debug[0..3]). BTW: I have no idea how to measure noise or tune a KF but I couldn't get the stripped down KF to work better than a lpf.
First of all you are right concerning the cascaded low pass filter. I observed on rapid barochanges an inverted filtersignal it's hard to see and rare but it is there - so that might cause trouble. At the end of the day I reworked the D part: Moving average removed, lpf added - 10Hz gives good results with my setup.
The mwii imu acc lpf part is reworked to proper lpf implementation now, finally got rid of that (flies nice with 1ms cycletime - though I blocked cycletime in cli.c to be lower than 3ms..).
- "acc_lpf" deleted and repaced by acc_lpfhz = 0.536Hz (that is equivalent to the former "100" based on 3ms cycletime)
- "acc_ilpf" deleted and repaced by acc_ilpfhz = 5.895Hz (that is equivalent to the former " 10" based on 3ms cycletime)
Some untested GPS changes (got rid of the bathtub stuff and introduced expo for gps angles to get also rid of that slew rate "delay" stuff)
EDIT: GPS PH tested. Awful because of wrong filter for D term, since the D term is most affected by noise. Change gpscuthz = 10 to gpscuthz = 5 (or maybe to some lower value) - that worked very well on my rig. The expo part seems to be a good way to get rid of that laggy slew rate stuff. I could reduce gps_ph_expo from 20 down to 12 for good results.

Cheers Rob

[Crashpilot1000]

Hi BRM!
Your noise stuff got me thinking.
I defined "noise" as something like this: When all is done/settled and calibrated and there is still some jitter around sensor zero going on.
The actual code/sensor calibration implements some noise measuring/simple cancelling for ACC and Gyro (from my debug datacurves I've seen that the acc desperately needs that - no matter what hardware LPF. 42, 100, 256 Hz watched - 256 Hz worse - of course). The noise seems to be quiet symetric around the axes (MPU6050/ACC/Gyro & MMA8452 watched) so I choose a single noise deadband value for acc and gyro - the gyro doesn't seem to really need it (but has it now in its' face - slam - just for the hell of it) at least at 42Hz LPF - acc seems to profit from "double averaged" deadband (tried 1.0x, 1.5x, 2.0x deadband, acc has some nasty spikes that are surprisingly not adequately represented in the average of 10000 values. - Doing a weighted filter or treat those spikes another way seemed to me/and my brain like overkill) - since the measurements are scaled down to mwii 512 the deadband boils down to a smaller factor - but helping the lpf/summation/ins stuff. Barofiltering also reworked (with a lot of testing/farting around) I am pretty sure BMP systems will see a nice boost there as well (ca. 5Hz more readout rate). Althold and PH are absolute killer now from what I've seen on my rig (cheap Dji 450 frame, 980KV motors @ 10'' props ca. 1KG weight, 3S 3300Lipo) during testing in windy condition (rescaled/put to work Pos I - extended gps tiltangle "gps_maxangle"). (Note: Althold tested with MS5611, my BMP085 system needs some assembly but will follow these days). Currently re-doing that IMHO fubar inflight ACC - calibration since it had it's useful days. I think - at the current state - the copter should be trimmed out in the air without recalibrating the acc since that will drive most likely havoc on the ins part (no correct 1G measurement, no acc noisemeasurement - I will not flight test it, since it's crazy from the start). While redoing that part I noticed that every trim stickinput (disarmed/throttleup + ail/pitch) will issue a separate write of the eeprom . Premature killing off the limited write ability (ca. 100000 times if I remember it right from the datasheet) with every trim stickinput of the STM eeprom to shorten its' life can be a reason or simply mindless copying? Honestly I have just been using that stuff on orig. mwii, a strict horizontally calibrated harakiri copter will hang exactly that horizontal in the air - end of story. If the ambient temp. is dramatically off the calibration temperature you may have to recalibrate it on the flat ground outside.
Well, the "pt1" element off topic goes on. I very appreciate your input but I am simply too dumb for that KF filtering stuff, I know "harakiri" has aquired some popularity but I retreated from the forums (except this and trolling on diy drones) to keep it small and private again. From my perspective the quanton flight control has the best price performace ratio and will beat TC crap hardware in every point. I think TC hardware should be boycotted by any means where possible because his latest fail - "design" doesn't have open hardwareplans and that is strongly against my beliefs of where should open software should run on. Even shabby pixhawk has a complete hardwareplan online - it's simply rediculous that TC sits on his plans in fear of clones protecting his profit. For me he became just a money greedy guy (re-inventing SimonK on arduino esc - where is the 32 Bit there? No time/knowledge/insight to copy Autoquad esc? etc..) including his resellers - and I am pretty sure he would close up everything if he could - but he is still profiting by contributing guys - hopefully they get a good share, otherwise they are plainly stupid - I know I am awfully wrong but that is how I see it and my point of view is enough for me.

Concerning this:

this must be true love.

if debugging and testing is the process to remove bugs, then writing software
must be the process creating them

no time for testing and bug catching - we are up in the mountains.
happy new year to all ...

Thank you very much! I hope you have a lot of Hüttengaudi!
Happy Xmas and happy new year!

Cheers
Rob

Edit: Just assembled and tested my BMP085 copter - "SGTodaysSnapshotFilterRework" works much worse on BMP - *fixed* - will upload new repo later.
Edit,Edit: Uploaded + Fix stupid eepromwrites on Trim