I am making an automated quadcopter: no radio transmitter-receiver and the quacopter flies on its own with pre-programmed orders.
All most all quadcopter implement PID on throttle/yaw/pitch/roll as these 4 axes are directly on an remote controller. However this is a bit inconvenient for an automated one without a controller. As an automated quadcopter without user input, velocities along x/y/z axis are of more concern, because:
keeping balance(yaw/pitch/roll=0) doesn't mean keeping still, as first, there will be some error in manufacture so it might still have some acceleration. Second, even there's no acceleration, it can have speed, causing it to drift in space. And as there's no user input, it can not fix the drift on it's own. Besides, if there's wind, it might got blow away, even it thinks it's balanced.
Orders are mostly given in "go to position(x,y)" or "keeping velocity x, fly above position(x,y) and start camera video capture." or so. These orders can't be translated into yaw/pitch/roll directly.
So basically I have two ideas:
Implement PID on yaw/pitch/roll/height and use a second PID loop to control velocity. The second PID loop take desired velocity and current velocity as input and output desired yaw/pitch/roll for first loop.
Implement PID directly on velocity. The pid loop take desired velocity and current velocity(by integrating acceleration from accelerometer) as input and output PWM width for 4 motors.
Have anyone tried idea 2? Will this work?
PID maps a measured value to a controlled value. If you can sense velocity reliably, you can use it to drive a PID. However, integrating an accelerometer won't give you a reliable enough velocity. Any sensing errors will compound through the integration and could grow your velocity estimate unbounded.
The R/P/Y PIDs on a quadcopter don't control the PWM to the motors directly, they control the roll, pitch, and yaw rates, then convert the rates to thrusts, then convert the thrust components for the various motors, then convert the thrusts into PWMs. See ArduPilor MotorMatrix code for hints.
You can put PIDs anywhere you like in the process between whatever is choosing the velocity and the motors, but you likely need some intermediate control/state variables between 'velocity' and 'PWM[1-4]' to balance things and have coordinated flight.
This is my first post on SO. I haven't already developed much code for embedded systems, but I have few problems and need help from more advanced programmers. I use following devices:
- LandTiger board with LPC1768 (Cortex M3) MCU,
- Digilent pmodACL with ADXL345 accelerometer (3 axis),
- Digilent pmodGYRO with L3G4200D gyroscope (3 axis).
I would like to get some information about device orientation, i.e. rotation angles over X, Y and Z axes. I've read that in order to achieve this I need to combine data from both accelerometer and gyroscope using Kallman filter or its simpler form i.e. complementary filter. I would like to know if it's possible to count roll, pitch and yaw from full range (0-360 degrees) using measurment data only from gyroscope and accelerometer (without magnetometer). I've also found some mathematical formulas (http://www.ewerksinc.com/refdocs/Tilt%20Sensing%20with%20LA.pdf and http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf) but they contain root squares in numerators/denominators so the information about proper quadrant of coordinate system is lost.
The question you are asking is a fairly frequent one, and is fairly complex, with many different solutions.
Although the title mentions only an accelerometer, the body of your post mentions a gyroscope, so I will assume you have both. In addition there are many steps to getting low-cost accelerometers and gyros to work, one of those is to do the voltage-to-measurement conversion. I will not cover that part.
First and foremost I will answer your question.
You asked if by 'counting' the gyro measurements you can estimate the attitude (orientation) of the device in Euler Angles.
Short answer: Yes, you could sum the scaled gyro measurements to get a very noisy estimate of the device rotation (actual radians turned, not attitude), however it would fail as soon as you rotate more than one axis. This will not suffice for most applications.
I will provide you with some general advise, specific knowledge and some example code that I have used before.
Firstly, you should not try to solve this problem by writing a program and testing with your IMU. You should start by writing a simulation using validated libraries, then validate your algorithm/program, and only then try to implement it with the IMU.
Secondly, you say you want to "count roll, pitch and yaw from full range (0-360 degrees)".
From this I assume you mean you want to be able to determine the Euler Angles that represent the attitude of the device with respect to an external stationary North-East-Down (NED) frame.
Your statement makes me think you are not familiar with representations of attitude, because as far as I know there are no Euler Angle representations with all 3 angles in the 0-360 range.
The application for which you want to use the attitude of the device will be important. If you are using Euler Angles you will not be able to accurately track the attitude of the device when large (greater than around 50 degrees) rotations are made on the roll or pitch axes, due to what is known as Gimbal Lock.
If you require the tracking of such motions then you will need to use a quaternion or Direction Cosine Matrix (DCM) representation of attitude.
Thirdly, as you have said you can use a Complimentary Filter or Kalman Filter variant (Extended Kalman Filter, Error-State Kalman Filter, Indirect Kalman Filter) to accurately track the attitude of the device by fusing the data from the accelerometer, gyro and a magnetometer. I suggest the Complimentary Filter described by Madgwick which is implemented in C, C# and MATLAB here. A Kalman Filter variant would be necessary if you wanted to track the position of the device, and had an additional sensor such as GPS.
For some example code of mine using accelerometer only to get Euler Angle pitch and roll see my answer to this other question.
I recently asked this question:
I am looking for an algorithm to detect pitch. one of the answers suggested that I use an initial FFT to get the basic frequency response, figure out which frequencies are getting voiced, and follow it up with a band pass filter in each area of interest:
A slightly advanced algorithm could do something like this:
Roughly detect pitch frequency (could be done with DFT).
Bandpass signal to filter isolate pitch frequency.
Count the number of samples between two peaks in the filtered signals.
Now I can do the first step okay ( I am coding for iOS, and Apple has a framework (the accelerate framework) for doing FFTs etc.
I have made a start here: but I can see the problem: an FFT that would differentiate all of the possible notes one could sing would require a lot of samples, and I don't want to perform too much unnecessary computation as I'm targeting a mobile device.
So I'm trying to get my head round this answer above, but I don't understand how I could apply the concept of a band pass filter to code.
Can anyone help?
Filter design is pretty complex. There are many techniques. First you have to decide what kind of filter you want to create. Finite impulse response (FIR)? Infinite impulse response (IIR)? Then you select an algorithm for designing a filter of that type. The Remez algorithm is often used for FIR filter design. Go here to see the complexity that I was referring to: http://en.wikipedia.org/wiki/Remez_algorithm
Your best best for creating a filter is to use an existing signal processing library. A quick Google search led me here: http://spuc.sourceforge.net/
Given what your application is, you may want to read about matched filters. I am not sure if they are relevant here, but they might be. http://en.wikipedia.org/wiki/Matched_filter
well in Wikipedia, checkup on low-pass filter, and hi-pass, then join them to make a band-pass filter. Wikipedia has code implementations for those two filters.
http://en.wikipedia.org/wiki/Low-pass_filter
http://en.wikipedia.org/wiki/High-pass_filter
Since you only want to detect a single frequency, it would be an overkill to perform a DFT to then only use one of the values.
You could implement the Goertzel algorithm. Like this C implementation used to detect DTMF tones over a phone line, from the FreePBX source code:
float goertzel(short x[], int nmax, float coeff) {
float s, power;
float sprev, sprev2;
int n;
sprev = 0;
sprev2 = 0;
for(n=0; n<nmax; n++) {
s = x[n] + coeff * sprev - sprev2;
sprev2 = sprev;
sprev = s;
}
power = sprev2*sprev2 + sprev*sprev - coeff*sprev*sprev2;
return power;
}
As you can see, the implementation is fairly trivial and quite effective for single frequencies. Check the link for different versions with and without floating point, and how to use it.
The sensor module in my project consists of a rotating camera, that collects noisy information about moving objects in the surrounding environment.
The information consists of distance, angle and relative change of the moving objects..
The limiting view range of the camera makes it essential to rotate the camera periodically to update environment information...
I was looking for algorithms / ways to model these information, in order to be able to guess / predict / learn motion properties of these object..
My current proposed idea is to store last n snapshots of each object in a queue. I take weighted average of positions and velocities of moving object, but I think it is a poor method...
Can you state some titles that suit this case?
Thanks
Kalman {Extended, unscented, ... } filters and particle filters only after reading about Kalman filters.
Kalman filters learn and predict the correct data from noisy data with a Gaussian assumption, so it may be of use to you. If you need non-Gaussian methods, look at the particle filter.
Not unlike a clap detector ("Clap on! clap clap Clap off! clap clap Clap on, clap off, the Clapper! clap clap ") I need to detect when a door closes. This is in a vehicle, which is easier than a room or household door:
Listen: http://ubasics.com/so/van_driver_door_closing.wav
Look:
It's sampling at 16bits 4khz, and I'd like to avoid lots of processing or storage of samples.
When you look at it in audacity or another waveform tool it's quite distinctive, and almost always clips due to the increase in sound pressure in the vehicle - even when the windows and other doors are open:
Listen: http://ubasics.com/so/van_driverdoorclosing_slidingdoorsopen_windowsopen_engineon.wav
Look:
I expect there's a relatively simple algorithm that would take readings at 4kHz, 8 bits, and keep track of the 'steady state'. When the algorithm detects a significant increase in the sound level it would mark the spot.
What are your thoughts?
How would you detect this event?
Are there code examples of sound pressure level calculations that might help?
Can I get away with less frequent sampling (1kHz or even slower?)
Update: Playing with Octave (open source numerical analysis - similar to Matlab) and seeing if the root mean square will give me what I need (which results in something very similar to the SPL)
Update2: Computing the RMS finds the door close easily in the simple case:
Now I just need to look at the difficult cases (radio on, heat/air on high, etc). The CFAR looks really interesting - I know I'm going to have to use an adaptive algorithm, and CFAR certainly fits the bill.
-Adam
Looking at the screenshots of the source audio files, one simple way to detect a change in sound level would be to do a numerical integration of the samples to find out the "energy" of the wave at a specific time.
A rough algorithm would be:
Divide the samples up into sections
Calculate the energy of each section
Take the ratio of the energies between the previous window and the current window
If the ratio exceeds some threshold, determine that there was a sudden loud noise.
Pseudocode
samples = load_audio_samples() // Array containing audio samples
WINDOW_SIZE = 1000 // Sample window of 1000 samples (example)
for (i = 0; i < samples.length; i += WINDOW_SIZE):
// Perform a numerical integration of the current window using simple
// addition of current sample to a sum.
for (j = 0; j < WINDOW_SIZE; j++):
energy += samples[i+j]
// Take ratio of energies of last window and current window, and see
// if there is a big difference in the energies. If so, there is a
// sudden loud noise.
if (energy / last_energy > THRESHOLD):
sudden_sound_detected()
last_energy = energy
energy = 0;
I should add a disclaimer that I haven't tried this.
This way should be possible to be performed without having the samples all recorded first. As long as there is buffer of some length (WINDOW_SIZE in the example), a numerical integration can be performed to calculate the energy of the section of sound. This does mean however, that there will be a delay in the processing, dependent on the length of the WINDOW_SIZE. Determining a good length for a section of sound is another concern.
How to Split into Sections
In the first audio file, it appears that the duration of the sound of the door closing is 0.25 seconds, so the window used for numerical integration should probably be at most half of that, or even more like a tenth, so the difference between the silence and sudden sound can be noticed, even if the window is overlapping between the silent section and the noise section.
For example, if the integration window was 0.5 seconds, and the first window was covering the 0.25 seconds of silence and 0.25 seconds of door closing, and the second window was covering 0.25 seconds of door closing and 0.25 seconds of silence, it may appear that the two sections of sound has the same level of noise, therefore, not triggering the sound detection. I imagine having a short window would alleviate this problem somewhat.
However, having a window that is too short will mean that the rise in the sound may not fully fit into one window, and it may apppear that there is little difference in energy between the adjacent sections, which can cause the sound to be missed.
I believe the WINDOW_SIZE and THRESHOLD are both going to have to be determined empirically for the sound which is going to be detected.
For the sake of determining how many samples that this algorithm will need to keep in memory, let's say, the WINDOW_SIZE is 1/10 of the sound of the door closing, which is about 0.025 second. At a sampling rate of 4 kHz, that is 100 samples. That seems to be not too much of a memory requirement. Using 16-bit samples that's 200 bytes.
Advantages / Disadvantages
The advantage of this method is that processing can be performed with simple integer arithmetic if the source audio is fed in as integers. The catch is, as mentioned already, that real-time processing will have a delay, depending on the size of the section that is integrated.
There are a couple of problems that I can think of to this approach:
If the background noise is too loud, the difference in energy between the background noise and the door closing will not be easily distinguished, and it may not be able to detect the door closing.
Any abrupt noise, such as a clap, could be regarded as the door is closing.
Perhaps, combining the suggestions in the other answers, such as trying to analyze the frequency signature of the door closing using Fourier analysis, which would require more processing but would make it less prone to error.
It's probably going to take some experimentation before finding a way to solve this problem.
You should tap in to the door close switches in the car.
Trying to do this with sound analysis is overengineering.
There are a lot of suggestions about different signal processing
approaches to take, but really, by the time you learn about detection
theory, build an embedded signal processing board, learn the processing
architecture for the chip you chose, attempt an algorithm, debug it, and then
tune it for the car you want to use it on (and then re-tune and re-debug
it for every other car), you will be wishing you just stickey taped a reed
switch inside the car and hotglued a magnet to the door.
Not that it's not an interesting problem to solve for the dsp experts,
but from the way you're asking this question, it's clear that sound
processing isn't the route you want to take. It will just be such a nightmare
to make it work right.
Also, the clapper is just an high pass filter fed into a threshold detector. (plus a timer to make sure 2 claps quickly enough together)
There is a lot of relevant literature on this problem in the radar world (it's called detection theory).
You might have a look at "cell averaging CFAR" (constant false alarm rate) detection. Wikipedia has a little bit here. Your idea is very similar to this, and it should work! :)
Good luck!
I would start by looking at the spectral. I did this on the two audio files you gave, and there does seem to be some similarity you could use. For example the main difference between the two seems to be around 40-50Hz. My .02.
UPDATE
I had another idea after posting this. If you can, add an accelerometer onto the device. Then correlate the vibrational and acoustic signals. This should help with cross vehicle door detection. I'm thinking it should be well correlated since the sound is vibrationally driven, wheres the stereo for example, is not. I've had a device that was able to detect my engine rpm with a windshield mount (suction cup), so the sensitivity might be there. (I make no promises this works!)
(source: charlesrcook.com)
%% Test Script (Matlab)
clear
hold all %keep plots open
dt=.001
%% Van driver door
data = wavread('van_driver_door_closing.wav');
%Frequency analysis
NFFT = 2^nextpow2(length(data));
Y = fft(data(:,2), NFFT)/length(data);
freq = (1/dt)/2*linspace(0,1,NFFT/2);
spectral = [freq' 2*abs(Y(1:NFFT/2))];
plot(spectral(:,1),spectral(:,2))
%% Repeat for van sliding door
data = wavread('van_driverdoorclosing.wav');
%Frequency analysis
NFFT = 2^nextpow2(length(data));
Y = fft(data(:,2), NFFT)/length(data);
freq = (1/dt)/2*linspace(0,1,NFFT/2);
spectral = [freq' 2*abs(Y(1:NFFT/2))];
plot(spectral(:,1),spectral(:,2))
The process for finding distinct spike in audio signals is called transient detection. Applications like Sony's Acid and Ableton Live use transient detection to find the beats in music for doing beat matching.
The distinct spike you see in the waveform above is called a transient, and there are several good algorithms for detecting it. The paper Transient detection and classification in energy matters describes 3 methods for doing this.
I would imagine that the frequency and amplitude would also vary significantly from vehicle to vehicle. Best way to determine that would be taking a sample in a Civic versus a big SUV. Perhaps you could have the user close the door in a "learning" mode to get the amplitude and frequency signature. Then you could use that to compare when in usage mode.
You could also consider using Fourier analysis to eliminate background noises that aren't associated with the door close.
Maybe you should try to detect significant instant rise in air pressure that should mark a door close. You can pair it with this waveform and sound level analysis and these all might give you a better result.
On the issue of less frequent sampling, the highest sound frequency which can be captured is half of the sampling rate. Thus, if the car door sound was strongest at 1000Hz (for example) then a sampling rate below 2000Hz would lose that sound entirely
A very simple noise gate would probably do just fine in your situation. Simply wait for the first sample whose amplitude is above a specified threshold value (to avoid triggering with background noise). You would only need to get more complicated than this if you need to distinguish between different types of noise (e.g. a door closing versus a hand clap).