This site documents Kalman filter applications by Duncan Rodriguez. They are meant to show the usefulness and application of such predictors for eventual use in more complex systems.

Pendulum Systems

Being able to sense information in a system is necessary in determining how that system is performing. If a car's engine is too hot then it is likely to stall and stop the car from moving. Engine temperature is therefore necessary to sense in the system to ensure proper function and know how engine RPM, vehicle velocity, and other factors would be affected.

The same is true for many other systems including vehicles such as quadrotors. If the copter's acceleration is unknown then it is impossible to know whether the vehicle is responding to inputs, or crashing. Additionally, drone technology and use of unmanned aerial vehicles is increasingly useful. It is becoming necessary for security, surveillance, reconnaissance, delivering aid, and other commercial, military, and humanitarian efforts; not to mention sport racing and photographic uses. This has led to a large amount of research in these areas and the desire for better control systems for these drones.

It is in this vein that I am producing proof-of-concept Kalman filters for eventual application in multirotors. By producing accurate predictors for a certain system it can be reasonably assumed that more complex systems can be implemented into the same algorithms. Thus a predictor for the states of that system can be developed.

Kalman filtering is an algorithm used to predict the states of a system. Where there is uncertainty in the sensor data there is potential to have an incorrect guess at the true state of the system. By using a Kalman filter it is possible to make accurate predictions of the true states of any well modeled system with noise.

In order to help with the task of producing accurate data for the predictors to use, it is useful to employ the idea of sensor fusion. Although many of the forces and accelerations in the system may be measurable from a single type of sensor, other sensors in different orientations can improve readings by compensating for forces that may be invisible or indistinguishable to that sensor. That is, if an accelerometer were to measure gravity in multiple axes to determine tilt/inclination it would begin to measure inaccurately once another force were to act upon it, i.e. push it to the side. A gyroscope could be used to fix this by integrating its signal to determine tilt and subtracting that from the accelerometer forces. The remainder would be the force of "the push" and we could know both the tilt and other forces acting on our sensors.

With all of these tools a good system predictor can be developed and used to control the system. Once the system is well controlled then it can be used to perform the desired tasks.

Since it is difficult to produce a physical system to test and control a quadrotor, I start by making proof-of-concept predictors that have easily built physical systems and can be used to help develop and show the usefulness of the more complex quadrotor system.

Pendulums were easy systems to build to a fairly accurate degree. The sensors used are a three axis accelerometer, ADXL345, and a three axis gyroscope, ITG3205.

An Arduino nano is used to read sensor values from an I2C bus that the ADXL345 and ITG3205 are on. When MATLAB code asks for a reading from the sensors, the Arduino quickly reads the values and sends them to the computer over USB.

I use two accelerometers and a gyroscope to determine the current angle and angular velocity of a 2-D pendulum. They are placed at the mass of the pendulum in order to measure the forces there as well as keep the center of mass lower. It is necessary to use a gyroscope to directly measure the angular velocity of the pendulum since it must be used to compensate the accelerometers. Both accelerometers are used to determine the angle of the pendulum.

Our gyroscope is then used to measure the angular velocity of the pendulum. The filter can use this as one of the states in our model to help better determine the angle of the pendulum.

All of this is combined into a system model which a Kalman filter uses to predict future states.

Free body diagram of a pendulum.

Free body diagram of a pendulum.

The system is first modeled using a static free-body diagram of a pendulum. This gives us the typical acceleration induced on the body by gravity and a second acceleration from the tensile string. Together these induce a net acceleration in the θ direction. θ is in the same direction as the y-axis accelerometer attached to the pendulum, thus the θ acceleration can be measured directly. R-axis acceleration is directly measured by the local z-axis of the accelerometer. Together these orthogonal accelerations, being components of gravity, allow us to determine the incline angle of the sensor and attached pendulum.

The two accelerometer readings are thus used with the assumption that their RSS value represents the current constant gravitational acceleration.

Two accelerometers are also used instead of one to alleviate error induced by the difficult assumption of the true current gravitational acceleration required for other trigonometric calculations of angle and noise allowing the measurements to randomly exceed the assumed gravity. These both have the effect of creating unknowable variation in the calculation of θ since they can randomly produce complex values, e.g. asin(10/9.8) = 1.57080... - 0.201688...i.

Free body diagram of a 3D pendulum.

Free body diagram of a 3D pendulum.

Adding an extra plane to create a 3D pendulum does not complicate the model since the planes are independent. Their independence is simply due to the fact that we are taking the local measurements and interpreting the global meanings. All of the global meanings remain the same since the local values of acceleration are changed proportionally during rotation in any of the two axes. Trigonometric functions are used to interpret the global meanings of the sensor data and are also inherent in the formation of the data, effectively canceling each other out and leaving the rotational axes independent.

Since the system is dynamic there are other forces acting on the sensors that need to be accounted for. Without these we will get inaccurate results for the above system.

We must consider that there is also an acceleration in the θ (instantaneous y-axis of the pendulum) direction proportional to gravity which affects the movement of the pendulum. This is α, the angular acceleration. Likewise, the r direction presents dynamic forces proportional to gravity, the centripetal force. In the dynamic system these forces are summed with the static model forces in the respective axes. This confounds the measurement results from using just the static model. We must therefore find these dynamical forces and remove them from our static model measurements to achieve an accurate measurement.

The Kalman algorithm and equations.

The Kalman algorithm and equations.

In a Kalman filter it is necessary to have noise in your measurements to run the algorithm. Without noise it is pointless to try predicting the states of the system since they would already be known. Thus it is necessary to tell the algorithm what the noise of the sensor and system are.

The algorithm requires two matrices which define the noise of the system and sensors, Q and R respectively. R is easy to determine since it is simply the variances and covariances of the sensors that are employed. Q is more difficult to determine but will be assumed to be similar to R.

These covariances must be determined to make R and Q for the algorithm. Finding sensor variances is as simple as recording a sting of data from the sensors analyzing the results. Each of the sensors was read for 200 'static' samples and then the variances were calculated. Since there was no physical movement the data should be centered about a mean value with some variance about that mean. This was indeed the case with the accelerometer data in all axes' having means around 10 m/s^2 with a variance of 0.036m/s^2. The gyroscopes were more accurate with 0 mean and 0.0022 rad/s variance. Both of these coincide with the datasheet values of ~1 LSB or 0.039m/s^2 for the accelerometers and 0.0066 rad/s for the gyroscopes.

Drift in the gyroscope measurements is a separate and more impactful issue than the variances. Sensor drift can induce overall state drift and render the filter essentially useless after a short while. However drift can be rather easily taken care of in a start-up calibration sequence.

Angle, angular velocity, and raw static model angle.

Angle (Magenta), angular velocity (Blue), and raw static model angle (Red).

The angular velocity is sinusoidal and has a very precise period, consistently around 23 samples at 62.4ms/sample This gives a period time of 1.4s which is close to the predicted 2π*sqrt(l/g) = 2π*sqrt(0.533/9.81) = 1.46s and 1.37s measured from timing 10 cycles. The discrepancy is due to the mass above the hanging mass which is not considered to calculate the effective pendulum length. The maximums seen are also as expected as well as the slow decay from the low friction system.

Measured 1.37s
((1.4 - 1.37)/1.37)* 100% = 2.2% error in period

Angle predictions are also roughly sinusoidal and have the same frequency, but are out of phase from the velocity by π/2 rad, as expected. The oscillation decay is also slow due to the low friction. Their amplitude is about 0.087 rad which is around 4.98 degrees, close to the measured maximum angle of 5.0 degrees by an external protractor.

Measured 0.087 rad = 4.98 degrees
((5.0 - 4.98)/5.0)* 100% = 0.4% error in angle

ω = sqrt{2gl^3*sin(θ)}
0.51 rad/s = sqrt{2*9.81*(0.533)^3*sin(0.087)} 5 degrees
Measured 0.49 rad/s
((0.51 - 0.49)/0.51)* 100% = 3.9% error in angular velocity

This high error is also due mainly to having samples from different time steps; angular velocity measurement being after the angle measurement and taking the 'true angle' from above as determining the 'true angular velocity'.


Results from portions of the 3D pendulum data also look good. The period is also around 23 samples.

((1.44-1.4)/1.44) * 100\% = 3% error in period

The angles and angular velocities are more difficult to precisely quantify error on but some rough measurements are below.

0.065 rad = 3.7 degrees
((4 - 3.7)/4)* 100% = 8% error in angle

3D pendulum angles and angular velocities moving majorly in the YZ-plane

3D pendulum angles and angular velocities moving majorly in the YZ-plane

This large error is due primarily to the inaccuracy of measurement of the 'known' angle and precision of the measurement.ω =

ω = sqrt{2gl^3*sin(θ)}
0.46 rad/s = sqrt{2*9.81*(0.533)^3*sin(0.070)} 4 degrees
Measured 0.41rad/s
((0.46 - 0.41)/0.46) * 100% = 11% error in angular velocity

This high error is also due to inaccuracy in measurements as well as having samples from different time steps; angular velocity measurement being after the angle measurement and taking the 'true angle' from above as determining the 'true angular velocity'.