How does Althold use the vertical accelerometer?

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
matt_prox
Posts: 18
Joined: Tue Sep 01, 2015 2:16 pm

How does Althold use the vertical accelerometer?

Post by matt_prox »

Apologies if this is the wrong place to post this question.

I am proficient in coding but by no means an expert. Looking at the code (in IMU.cpp, MW version 2.3), I see that the barometer is used with a simple low pass filter:
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)


Later, I see that a complimentary filter is used to combine the vertical velocity, calculated from differentiating the baro readings, with that calculated by integrating the vertical accelerometer reading, to better estimate actual vertical velocity:
vel += accZ * ACC_VelScale * dTime;

// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * 0.985f + baroVel * 0.015f;


However, the complimentary filter seems to only be used for the velocity term (i.e. the D term of the PID controller). Is does not seem to be implemented for the alt.EstAlt variable, which is used to calculate error for the P and I terms of the althold control loop. As I understand things, alt.EstAlt seems to be based solely on the baro reading. Is this correct? If so, the P and I terms do not use accelerometer readings at all, only the D term does. Why not implement a similar complimentary filter for P and I too?

I am thinking about experimenting with rewriting the althold algorithm, starting by using a complimentary filter to estimate altitude by combining readings from both sensors, and only applying PID control later. I am also thinking about including a subroutine to check if the sonar is in range, and if it is then use this value instead of the baro to calculate alt.EstAlt. I will report back with how it goes - but first I need to work out how to query the sonar reading from the i2c module that it is connected to. So many fun projects!

Matt

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: How does Althold use the vertical accelerometer?

Post by edsimmons3 »

Hi Matt,

I think you're right, the alt.EstAlt is derived only from the baro. I have noticed the altitude reported in the GUI is very noisy (slow sinusoidal movement) but generally quite accurate on a scale of several meters. I think complimentary filtering would be good...

The same approach used for the velocity term is probably sufficient but I'd have to try it out.

Why not make a github branch and test it out? I'll have a go too...

If you're up for some fun testing too, please feel free to try out the GPS Failsafe RTH fork: https://github.com/estechnical/multiwii-firmware

Happy hacking :-)
Ed

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: How does Althold use the vertical accelerometer?

Post by edsimmons3 »

Also, the Z accel varies with the cosine of the pitch/roll. So if you feel particularly fancy, de-tilt your accel value with pitch & roll.

;-)

matt_prox
Posts: 18
Joined: Tue Sep 01, 2015 2:16 pm

Re: How does Althold use the vertical accelerometer?

Post by matt_prox »

Thanks Ed,

I see that in the original code there is an algorithm to correct for pitch/roll already, to work out the true vertical component of the acceleration. So I'll just copy that! I will definitely be testing the new complimentary filter method out but, as I said, I am not a coder so I will probably just develop the algorithm on my own and find out how to post to Github once it is done (if it actually works and shows a clear improvement on the original).

PS - Since posting earlier today, I have managed to get my sonar running and if it detects an altitude of less than 2m, it is used instead of the barometer - obviously giving much more stable altitude estimates. With a little tuning, I can should be able to get very stable alt hold below 2m (and over hard ground) with this method alone.

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: How does Althold use the vertical accelerometer?

Post by edsimmons3 »

Why not just post snippets here? I can unofficially integrate things into my own forks on github which I'm hoping can be reviewed and included at some point.

I just coded this in the existing uint8_t getEstimatedAltitude() method...

Code: Select all

static float edAlt = 0.0f; // added for testing
uint8_t getEstimatedAltitude(){
    ... // snip
    // Integrator - velocity, cm/sec
    vel += accZ * ACC_VelScale * dTime; // < this line was already there, I placed my code after it

    #if defined(DEBUG)
    // ed's complimentary filtering of the baro and Z accel
    debug[1] = vel;
    edAlt =  0.95f * (edAlt + vel) + 0.05f * alt.EstAlt;
    debug[2] = edAlt;
    #endif
    ... // more snip


There is a horrid impact on loop time, but I'm not surprised with all that float multiplication - now the trick is to see if a) it is an improvement, b) it's worth the time penalty, c) how it can be optimised...

It gives a good result in my very basic bench testing. I have a multiwii mega board plugged in and I'm waving it about! It tracks the fast changes very well and overall follows the baro altitude as expected. This is a visual improvement at very least. I don't know how much impact it will have overall, but my experience with alt hold has been that P term is very slow to take effect and an input with a closer match to small/fast movements has to be good here.

Great re sonar :) Get it ground hugging ;)

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: How does Althold use the vertical accelerometer?

Post by edsimmons3 »

On the subject of altitude control, have you seen Lidar Lite? I *really* want one of these!

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: How does Althold use the vertical accelerometer?

Post by edsimmons3 »

I have to think about this more, it feels late here and I should sleep really.... but... I think that the input altitude value being improved like this negates the need for the D term complimentary filter and a velocity obtained from thisVal - lastVal could be used. This probably brings the loop time back in line with previous performance and hopefully speeds up P term response. What do you think?

matt_prox
Posts: 18
Joined: Tue Sep 01, 2015 2:16 pm

Re: How does Althold use the vertical accelerometer?

Post by matt_prox »

You may well be correct there. I agree, it's late and I need to sleep too. Originally, I envisioned the algorithm should be as follows:
- read baro, yields Altitude_baro (prone to noise)
- apply double integral on the acc data, yields Altitude_acc (prone to drift due to cumulative errors from the integration approximation)
- complimentary filter to combine Altitude_Acc and Altitude_baro to get more accurate altitude estimate
- PID control with this altitude estimate as a process variable, to finally get BaroPID

How this can be optimised in terms of float mathematics, etc, is beyond me right now. I'll have to think about it too.

I can definitely post snippets here as I improve on my code. For now, here is my addition to use sonar where available. I replaced this line:

Code: Select all

alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)


with the following:

Code: Select all

  AltSonar = i2c_readReg(I2C_GPS_ADDRESS,I2C_GPS_SONAR_ALT); //reads i2c sonar
 
  if (AltSonar <200)  //if in range (<2m), use sonar. Else, use baro.
    alt.EstAlt = AltSonar;
  else
    alt.EstAlt = (alt.EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
  }


Pretty simple, really. I'm paying around with ways to select options using the aux inputs (no alt hold vs. baro control vs. baro+sonar control).

My project is a little different than most, in that I'm using custom Rx units (using NRF24l01+ modules), so my loop time is very inconsistent (it spikes if there is a radio packet available to read). Thus, integrating the acc readings over time produces very unreliable vel values. In the original code, the integral uses the time period from the previous cycle, but with the *current* cycles acc data - this works fine if the cycle time is more or less consistent. Because mine isn't, it's affecting the whole AltHold algorithm. It is also causing twitches in flight. I may not be able to solve this until I can get proper hardware (or at least rig up a secondary arduino to offload the incoming Rx data handling), and I definitely can't solve it now, it's late here too.

Anyway, I'll keep posting here as I find things. Thanks for the help!

matt_prox
Posts: 18
Joined: Tue Sep 01, 2015 2:16 pm

Re: How does Althold use the vertical accelerometer?

Post by matt_prox »

Off the top of my head, I agree with you about neglecting the D-term complimentary filter. The (thisVal - lastVal)/time for the D term would probably be enough to stabilize the P and I terms so that higher P and I values can be used (the ultimate goal of the D term). Essentially, I am not suggesting that we should apply more complimentary filters to the existing algorithm. Rather, I am suggesting to moving the complimentary filter to before the P and I calculations were done, so that they are performed on more accurate estimates of altitude.

With that said, I'm by no means an expert in any of these things. But I'm finding it fun to learn...

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: How does Althold use the vertical accelerometer?

Post by edsimmons3 »

Just out of interest what rate are you sending packets and how long does it take to parse it? Can't you round-robin schedule responding to the packet in smaller steps similar to GPS data so it takes a few iterations of the loop and doesn't wreck the cycle time?

The Z accel is already being integrated twice in my implementation...

First Z accel velocity is determined, then this is multiplied by dt to get a distance traveled, then added to the existing altitude... I don't see much difference in these approaches. The risk with keeping a pure integral of the Z accel is that it can quickly run away to be very large or very negative given a consistent non-zero input value. What deadband is correct for this, without losing small change response... agh, real world data!

Complete guess: Perhaps a leaky integrator would provide better results, it would need no deadband and would only be successful at keeping track of short term changes, which is probably all we want. Then complimentary filtering the output of the leaky integrator and the baro would give similar results without the risk of huge runaway integrals.

I'd agree that computing the complimentary filter before the PID was a good thing, then the whole PID benefits from a smoother and more accurate altitude.

matt_prox
Posts: 18
Joined: Tue Sep 01, 2015 2:16 pm

Re: How does Althold use the vertical accelerometer?

Post by matt_prox »

My radio modules are transmitting at 150Hz. The air data transfer rate is supposed to be 2mbps, and my data packets are only 16 bytes (8 channels, 2 bytes each) plus a few bytes in the acknowledgement packet. There will be some overhead with checksums, etc, but ultimately I think the majority of the delay comes from parsing. I will look into the round robin schedule system, thanks for that. I'm not sure it will help in this instance, but it seems like a useful trick to know. I will probably end up building a simple promini-based rf receiver with the NRF24L01+ modules.

I agree that there's no way around the double integral and that this will compound the error. Some amount of error accumulation is obviously to be expected. Whether it will become too much will require testing. I'll give it a try this weekend. My sleepy brain last night thought you were suggesting a second complimentary filter, which would be unnecessary.

I don't know anything about leaky integrator implementations, so I'll need to research those too. I fear that you have hit the limits of software development knowledge for this lowly mechanical engineer! I am learning a lot of neat tricks - like multiwii's method for removing the Z offset from the accelerometers using only bitshifts (I have included the comments for my own understanding of the algorithm):

Code: Select all

  
static int16_t accZoffset = 0;
  if (!f.ARMED) { //when on the ground
    accZoffset -= accZoffset >> 3; //accZoffset subtracts 1/8th of itself then adds accZ (re-read each cycle)- thereby, after enough loops it approaches the value of 8*accZ
    accZoffset += accZ;
  }

//subtract 1/8th of accZoffset from the raw sensor value.
//Since accZoffset is 8*accZ when on the ground, the offset at zero acceletation (i.e offset error).
  accZ -= accZoffset >> 3;

  applyDeadband(accZ, ACC_Z_DEADBAND);

edsimmons3
Posts: 100
Joined: Thu Sep 10, 2015 11:29 am

Re: How does Althold use the vertical accelerometer?

Post by edsimmons3 »

How long can parsing 16 bytes take? Checksums can be costly if not implemented really efficiently...

I think the nice thing about the complimentary filter working with the integrated distance (calculated only for the current cycle's data) being added to the filter total means we haven't got a runaway value we have to keep in check, the decay of the filter keeps the value tracking the baro in the long term.

I plan to have another look at this soon, I hope to adjust the implementation of the Alt PID so it's using the new altitude value as an input for all the terms.

There are a few things to look still... for example, my filter implementation that I hurriedly wrote the other day exhibits a few large swings before slowly settling to the correct value when first powered on. I'm not sure whether I'm getting data that's showing this too, I haven't looked carefully enough yet!

Today brings some weird deliveries for work and fun, so I might get some time hacking on this while waiting for things to arrive...

I tried the leaky integrator idea on the double-integral accZ values, it's not as effective as just the complimentary filter, and of course incurs a time penalty. Worth a look though! They're really simple, instead of integrating like

Code: Select all

integral += newValue;


it's like

Code: Select all

integral = (integral * 0.99f) + newValue;

where changing the 0.99f controls the 'rate of leak'. It's a high pass filter of sorts.

Ed

Post Reply