I am in the process of constructing a vertical take-off and landing vehicle which utilizes a 6-solenoid carbon dioxide Attitude Control System.
For my initial set-up, i am only interested in controlling the 3-rotational degrees of freedom. I am using an Arduino Uno coupled to the MPU-6050 gyro.
I have already read all the threads in terms of programming the MPU-6050 and using Kalman filter. There are several questions that have been left un-answered and I would kindly request that other users with knowledge in the area can provide me with some guidance.
After I activate the DMP (Digital Motion Processor) and extract the quaternion, there are two ways to go about it. I can use an "Unscented Kalman Filter" directly on the quaternion and then move on finding my "ACS reaction vector". The other way is I could also resolve the quaternion into pitch, yaw, and roll and then apply the most simple Kalman filter separately to each angle. What is the best way to go about it?
Once I calculate the reaction vector, I need to obtain the right combination of ACS thruster bursts. The ACS needs to balance any external forces, remove unwanted rotation, and return the vehicle to its original orientation. What could be a general structure of this code, in other words, do I set "if statements" in relation to current acceleration, velocity, or angle?
Given that you're tackling it, I assume you know what Kalman does and its role in the solution. In general I would normally expect to feed the whole sensor input to the Kalman filter and resolve all the modes at the same time but I haven't ever implemented that in a microcontroller so I don't know whether there are any practical issues which require a different approach. Google shows various projects applying Kalman filters to this IMU so perhaps further reading would be useful - for example, https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050.
Given that you want to control movements around three axes I would guess that a PID for each degree of freedom would be the way to go. It would be worth your time to research the control algorithm used on multirotor UAV projects to see whether they have found a better approach.
If you aren't committed to implementing all this for yourself, I think you could save a huge amount of effort by adopting one of the open source UAV autopilot projects as the starting point for your project.
That sounds like a great idea. I will look into using a UAV autopilot as a starter.
The only issue I foresee with doing that, is that 99% of UAV are rotor-craft, so for these vehicles attitude control will stem from controlling the RPM of the various rotors. I cannot imagine at this point how difficult it would be to substitue RPM control algorithms on rotors to Solenoid-activation for the cold gas co2 system in my case.
These days a large proportion of hobby UAVs seem to be multirotor prop hangers and the open source projects have been designed to make it easy to cope with variations in the number/geometry of lift motors - I think your solution will fit in very easily. In any case, once you have calculated the demand for net thrust and torque in each axis, implementing that by distributing the thrust among a set of impulse jets is really no different to moving flight control surfaces - the hard part is the number crunching to determine the demands.
I have looked thoroughly through autopilot UAV and they all use PID control algorithms. Clearly, I would like to use a Kalman filter and re-writing PID algorithms into a Kalman filter to me sounds like a waste of time.
However, I did find Kalman and quaternion codes separately and will be integrating the two. Once I finish with the code, would you have a few minutes to glance over it and give me some feedback?
jremington:
PID algorithms and Kalman filters are completely unrelated and used for very different purposes.
They're different in the sense that a Kalman filter is a way of analysing sensor readings from multiple related sensors and modeling the behaviour of the system that produced those sensor readings; PID is a closed loop feedback control algorithm that can be used to drive the state of a system to a desired state. They're related by the fact that to fly a drone you need to do both of these things.
The fact that Kalman filters and PID control algorithms are generally unrelated in theory and application is not an "opinion". However, I agree with Peter that to fly a UAV you need to do both.
The Wikipedia articles on PID control theory and Kalman filters provide fairly reasonable overviews and are recommended reading. There are much better expositions of both topics that I'll don't have at my fingertips, but I would be happy to dig them up, if you are interested.
Ok, I am beginning to get a better understanding. However, after looking through a number of UAV autopilot codes and other software developed, I cannot find a single example that performs both Kalman Filtering and then engages various PID's.
So the idea is to have the Kalman Filter predict the state of the vehicle in terms of Pitch,yaw,roll for timestep t+1, then the PID algorithms take this prediction and engage the solenoids in a particular configuration to achieve this state. Is this a correct understanding of how the two are related to each other?
I haven't looked inside these autopilot programs to see what sort of closed loop control algorithms they use. Maybe it's something more sophisticated than a PID per degree of freedom. However, there will be a closed loop control algorithm of some sort in there.
I still think that adapting an existing autopilot to your IMU type and thruster/actuator configuration will be massively easier than writing your own controller from scratch.
I recommend some study of the Arducopter code, as it is directly relevant to your project. Download the entire source and have a look in the APM_Control directory. There you will find the individual modules that implement PID control of the pitch, yaw, etc.
For example here is the code for the pitch control -- short and sweet, although I object to the appearance of unexplained mystery constants like 4500.
Edit: the version of Arducopter with which I am most familiar does not use a Kalman filter for attitude estimation, instead it uses a DCM algorithm, see AP_AHRS_DCM.cpp.
would you be kind enough to throw some comments in this code? I have no absolute clue whats going on! I have a sense that the last section called "stabilize" has something to do with actually engaging the servos...
You should be directing your questions to the developers and users of the software.
There is an active forum at http://copter.ardupilot.com/
However the code I posted is straightforward. It implements the standard PID algorithm with lots of checks for out of range values, and a few options. The procedure simply returns the control output for the pitch angle. It does not control anything, because there are other corrections to be made for roll, yaw, etc.
int32_t angle_err = angle - _ahrs->pitch_sensor; //error in pitch, in later lines corrected for inverted flight
...
float rate = _ahrs->get_pitch_rate_earth(); //rate of change of pitch in earth frame
...
rate = _last_rate +
(delta_time / (RC + delta_time)) * (rate - _last_rate); //low pass filter on rate of change of pitch
...
int32_t desired_rate = angle_err * _kp_angle; //self explanatory except for variable name
...
float out = constrain(((rate_error * _kp_rate) + (desired_rate * _kp_ff) + roll_ff) * scaler,-4500,4500); //algorithm output, corrected for roll, if "stabilize" option is set.
...
_integrator += (rate_error * _ki_rate) * scaler * delta_time; //Ki term, integrated over time
...
return out + _integrator; //optional output if the "stabilize" option is not set
My friend and I began writing the code yesterday. Here is what we did so far (I know it is unfinished). If you guys have any comments please feel free to chime in!
As our base, of course, we began with the open-source example provided by the Arduino community: mpu6050-dmp6. For now we will neglect linear acceleration along the three axis, as this type of acceleration is associated with harmless drift. Yaw, pitch, and roll will be the primary concern at this moment, as they are responsible for attitude.
For our application, the mpu6050 is mounted horizontally along the mid-section of the fuselage. Roll, in the sense of the Airbooster-1, will thus occur as rotation in the plane of the mpu6050 chip. Hence, we have to switch yaw and roll to obtain a proper representation of our rotation.
The two decimal places on the y,p,r output array appear fairly unstable. So we truncate the decimal places and leave only whole numbers. The output array is thus in integer degrees, and may be easily zeroed in. Hopefully, this somehow helps with future programming. At least for now, it helps with visualizing the array.
The mpu6050 does not have a consistent sample rate. The sample rate changes between 7ms and 9ms. We wrote out a new variable dt which tracks down the instantaneous time-increment between every sample. Let’s note that the average clock-speed is thus about 143Hz, and the reported speed on most i2c forums is around 200Hz. Also, we note that when the mpu6050 is un-touched, the processing speed reaches its maximum which is about 1kHz. On the other extreme, when the mpu6050 undergoes extreme shaking, the processing speed drops to as low as 10Hz and the FIFO buffer overflow error comes up and stops the process.
We declare a set of new variables to store the y,p,r values from the previous time-step. We also declare velocity variables. These are calculated as the derivatives of the y,p,r, using the instantaneous time increment dt. The three velocity components vy,vp,vr are printed out in an array along with the y,p,r values. These velocity values can then be used in the future to calculate accelerations, body-intertia, etc… We note that the code registers velocity values of 100 degrees/second and above. For some reason, values below 100 degrees/second are not currently being registered.
For your reference, I have included the most recent photo of the machine. You can clearly see the two solenoid-valves on each side of the apparatus. There are two more, aft facing, on the back. (3 solenoids on each side)
