Third Year Project Report

THIRD YEAR MEng PROJECT
‘ ”Smart” Spoon to Compensate for Parkinson’s
Tremors’
Kyle Birtwell
Supervisor: Rosa Letizia
June 2016

2
Lancaster University
Faculty of Science and Technology
Engineering Department
Signed Declaration on the Submission of a Project
I declare that this project is my own work and has not been submitted in substantially
the same form towards the award of a degree or other qualificatory work, and affirm
that acknowledgement has been made to assistance given and that all major
sources have been appropriately referenced.
Signed: Kyle Birtwell
Date: 25/05/16

3
1.0 Summary
Many people across the world suffer the neurological degenerative disease,
Parkinson’s, causing uncontrollable tremors, as well as other side effects. Many
studies have been conducted to try to aid the tremors and help patients, however
there is yet no absolute cure for the disease. Assistive technologies are currently
used to help patients but these technologies can only help to a certain degree and
can be expensive.
The aim of this project is to research and test a technological assistive spoon, known
as the “smart” spoon, and find out if it is possible to develop this product using a
servo motor and microcontroller, for a fraction of the current market price. Liftware, a
Google company, has developed the current “smart” spoon on the market, of which
there is no documentation available to view on its development or the technology
used, thus this project uses concepts and ideas developed from numerous research
papers and resources.
When a tremor occurs it is completely random and unpredictable, therefore the
“smart” spoon must be able to react in real-time in order to compensate for the
tremor effectively. Real-time technologies must have an extremely fast processing
time which means they need a fast processor and a code that is simple enough to
compute in a short amount of time, therefore the microcontroller, programming
structure and programming language must be adequate, for these reasons the
Arduino series of microcontrollers are suitable, further specifications are listed in the
literature review.
A person suffering from Parkinson’s disease will need to make voluntary movements
when eating or doing other activities, for this reason the “smart” spoon must be able
to differentiate between voluntary movements and a tremor. Using algorithms and
setting a frequency threshold, a point whereby the movement is of a certain
frequency and therefore is recognised as a tremor, the “smart” spoon is able to do
this.

4
Table of Contents
1.0 Summary ............................................................................................................. 3
1.1 List of Abbreviations and Figures .................................................................... 6
1.2 List of Equations ................................................................................................ 8
2.0 Introduction....................................................................................................... 10
3.0 Literature Review.............................................................................................. 11
3.1 Introduction ............................................................................................................... 11
3.2 Programmable Controllers....................................................................................... 12
3.21 Programmable Controllers in Industry .................................................................. 12
3.22 Small Scale Open Source Programmable Controllers.......................................... 14
3.23 Arduino.................................................................................................................. 15
3.24 Raspberry Pi ......................................................................................................... 17
3.3 Sensors and Transducers........................................................................................ 20
3.4 Measuring Movement ............................................................................................... 20
3.41 Accelerometers ..................................................................................................... 21
3.42 Gyroscopes........................................................................................................... 22
3.43 Magnetometers ..................................................................................................... 22
3.5 Counteracting Movement......................................................................................... 23
3.51 Inputs .................................................................................................................... 23
3.52 Filtering and Processing ....................................................................................... 23
3.53 Outputs ................................................................................................................. 25
3.6 Conclusion ................................................................................................................ 25
4.0 Filters................................................................................................................. 26
4.1 Average Filter............................................................................................................ 26
4.2 Moving Average Filter .............................................................................................. 28
4.3 Low-Pass Filter ......................................................................................................... 30
4.31 First Order Low-Pass Filter ................................................................................... 30
4.4 The Kalman Filter...................................................................................................... 32
4.5 Complimentary Filter................................................................................................ 37
5.0 Components...................................................................................................... 39
5.1 Microcontroller.......................................................................................................... 39
5.11 Arduino Nano ........................................................................................................ 39
5.2 Input Sensor.............................................................................................................. 41

5
5.21 MPU-6050............................................................................................................. 41
5.3 Servo Motor............................................................................................................... 43
5.31 SM-S2309S Servomotor ....................................................................................... 43
5.32 HKSCM9 Servomotor ........................................................................................... 44
6.0 Building the “Smart” Spoon............................................................................ 45
6.1 Setting up .................................................................................................................. 45
6.2 The Kalman Filter Code............................................................................................ 49
6.21 The System State xk............................................................................................. 49
6.22 The measurement zk ............................................................................................ 51
6.23 Prediction .............................................................................................................. 52
6.24 Update the System ............................................................................................... 53
6.25 The Kalman Gain .................................................................................................. 54
6.3 Implementing the Filter ............................................................................................ 55
6.4 Arduino Libraries ...................................................................................................... 60
6.5 Kalman Filter Library ................................................................................................ 60
6.51 Kalman Filter Header File (.h)............................................................................... 61
6.52 Kalman Filter Source File (.ccp)............................................................................ 63
6.53 Keywords File ....................................................................................................... 68
6.54 I2
C Arduino File (.ino)............................................................................................ 69
6.6 MPU-6050 Arduino Code.......................................................................................... 71
6.61 Side Note on Arrays.............................................................................................. 85
6.62 Continuing with the Code...................................................................................... 86
6.7 MPU-6050 Arduino Code Flow Diagram ................................................................. 95
7.0 Testing the “Smart” Spoon ............................................................................. 96
8.0 Conclusion Upon Testing .............................................................................. 103
9.0 Conclusion of Project .................................................................................... 103
10.0 References .................................................................................................... 105
11.0 Appendices ................................................................................................... 110
11.1 Appendix A – Tilt Sensing Using a 3 Axis Accelerometer................................ 110
11.2 Appendix B – MPU-6050 Registers ..................................................................... 115
11.3 Appendix C - SM-S2309S Data Sheet.................................................................. 118
11.4 Appendix D – MPU-6050 Code by Kristian Lauszus.......................................... 119

6
1.1 List of Abbreviations and Figures
Figure 1: The PLC System [14] ................................................................................ 13
Figure 2: Decline in cost of Information Technology from 1960-2015 [17]............... 14
Figure 3: Arduino Uno Diagram [15]......................................................................... 15
Figure 4: Table of Arduino Uno Specifications [21] .................................................. 17
Figure 5: Raspberry Pi Labelled Diagram [23] ......................................................... 18
Figure 6: Flow Diagram of “Smart” Spoon operation................................................ 19
Figure 7: Seismic mass and spring accelerometer diagram [24].............................. 21
Figure 8: Vibrating Wheel Gyroscope Diagram [31]................................................. 22
Figure 9: Kalman filter algorithm [42]........................................................................ 32
Figure 10: Table of Kalman Filter Algorithm Variables [42]...................................... 33
Figure 11: Normal distribution of error covariance ................................................... 36
Figure 12: Complimentary Filter Block Diagram [42]................................................ 38
Figure 13: Arduino Nano Front [44].......................................................................... 40
Figure 14: Arduino Nano Back [44] .......................................................................... 40
Figure 15: Arduino Board Comparison [21].............................................................. 40
Figure 16: Arduino Nano Pin Diagram [45]............................................................... 40
Figure 17: MPU-6050 [48] ........................................................................................ 41
Figure 18: Arduino Uno MPU-6050 Wiring [51]........................................................ 42
Figure 19: SM-S2309S Specifications [52]............................................................... 43
Figure 20: HKSCM9 Specifications [53] ................................................................... 44
Figure 21: Arduino Uno – MPU-6050 setup ............................................................. 45
Figure 22: Code for Raw Data Values [54]............................................................... 46
Figure 23: MPU-6050 Data Registers [56] ............................................................... 47
Figure 24: Global Variables...................................................................................... 47
Figure 25: Setup Definitions ..................................................................................... 48
Figure 26: MPU-6050 Arduino Loop......................................................................... 48
Figure 27: Serial Monitor of LED Testing ................................................................. 72
Figure 28: Roll, Pitch, Yaw on a plane [62] .............................................................. 72
Figure 29: Roll, Pitch, Yaw Diagram [63].................................................................. 72
Figure 30: Orientation of Axes of Sensitivity and Polarity of Rotation ...................... 73
Figure 31: Bitwise Shift Operator [65]....................................................................... 81
Figure 32: Bitwise OR Operator [65] ........................................................................ 81

7
Figure 33: Arduino Serial Monitor “smart” spoon code............................................. 93
Figure 34: Arduino Uno Servo Test.......................................................................... 96
Figure 35: Arduino Nano Servo Test........................................................................ 97
Figure 36: Arduino Nano Veroboard......................................................................... 98
Figure 37: 2 Cell Battery........................................................................................... 99
Figure 38: Potentiometer Processing Graph .......................................................... 100
Figure 39: Arduino Code for Processing ................................................................ 101
Figure 40: Processing Code to Plot Graph............................................................. 102
Figure 41: Table of Costs ....................................................................................... 104

Abbreviations:
• LED – Light Emitting Diode
• IMU – Inertial Movement Unit
• IDE – Integrated Development Environment
• HRQOL - Health and Heath-Related Quality of Live
• PLC – Programmable Logic Controller
• PD – Parkinson’s Disease
• IC – Integrated Circuit
• PTH – Plated Through Hole
• SMD – Surface Mount Device
• PWM – Pulse Width Modulation
• FES – Feedback-controlled Functional Electrical Stimulus
• MEMS – Micro Electro-mechanical System
• WFLC – Weighted Fourier Linear Combiner
• BMFLC – Band Limited Multiple Fourier Combiner
• SCL – Serial Clock Line
• SDL – Serial Data Line
• ms – Milliseconds
• PCB – Printed Circuit Board
• DOF – Degrees of Freedom

8
1.2 List of Equations
Equation 1.1: Average “Batch Expression” [42]....................................................... 26
Equation 1.2 [42] ...................................................................................................... 27
Equation 1.3: Average Recursive Expression [42] .................................................. 27
Equation 1.4: Average Filter [42]............................................................................. 28
Equation 2.1: Moving Average [42] .......................................................................... 29
Equation 2.2 [42] ...................................................................................................... 29
Equation 2.3: Moving average filter (Recursive form) [42] ....................................... 29
Equation 3.1: 1st
Order Low-Pass Filter (Recursive Form) [42]............................... 30
Equation 3.2 [42] ...................................................................................................... 31
Equation 3.3 [42] ..................................................................................................... 31
Equation 6.1: Kalman Filter estimate [42]................................................................. 34
Equation 6.2 [42] ...................................................................................................... 34
Equation 6.3 [42] ...................................................................................................... 34
Equation 6.6: Error Covariance and Estimate .......................................................... 36
Equation 7: System State [57].................................................................................. 49
Equation 7.1: State Matrix [57] ................................................................................. 49
Equation 7.3: State Transition Matrix [57] ................................................................ 50
Equation 7.4: System State with rate [57] ................................................................ 50
Equation 7.5: Control Input Matrix [57]..................................................................... 50
Equation 7.6: Process Noise Equation [57].............................................................. 50
Equation 7.7: Covariance Matrix [57] ....................................................................... 51
Equation 7.8: Observation of true state [57]............................................................. 51
Equation 7.9: R Equation [57].................................................................................. 52
Equation 8: Control Input [57]................................................................................... 52
Equation 8.1: Error Covariance Equation [57].......................................................... 52
Equation 8.2: Error Covariance Equation [57].......................................................... 53
Equation 8.3: Innovation Equation [57]..................................................................... 53
Equation 8.4: Innovation Equation 2 [57].................................................................. 53
Equation 8.5: Innovation Covariance [57]................................................................. 53
Equation 8.6: The Kalman Gain [57] ........................................................................ 54
Equation 8.7: The Kalman Gain Matrix [57].............................................................. 54
Equation 8.8: Posteriori Estimate of Current State [57]............................................ 54

9
Equation 8.9: Update Posteriori Error Covariance Matrix [57].................................. 55
Equation 9: Control Input Matrix [57]........................................................................ 55
Equation 10: Pitch Equation [64] .............................................................................. 82
Equation 10.1: Roll when Pitch is Restricted [64]..................................................... 82
Equation 10.2: Unrestricted Pitch [64]...................................................................... 82
Equation 10.3: Roll when Pitch is Unrestricted [64].................................................. 83

10
2.0 Introduction
The entirety of this report aims to explain all processes taken for each step in the
development of the “smart” spoon as stated in the project brief. The layout of this
report will be:
• Research and literature review – This section builds up a good understanding
of the previous and current research being conducted and was required to
gain enough knowledge to be able to make decisions on the design of the
“smart” spoon.
• Methods of programming, including filtering – The programming code will be
explained in this section for a simple understanding of what the code means
and what each section of the code does. The applied filter will be explained
and compared throughout this section with the aim of providing the reader
with a basic understanding of how the filter works and what the benefits are of
using such filter.
• Building and testing – This section of the report will provide all relevant
information of the building process, from materials and products used,
prototyping, to the final product. Problems encountered during the build will be
explained and how the problems were solved.
• Conclusion and improvements – The final part of the report underlines and
summarises the project as a whole, giving a verdict on how the project has
succeeded in meeting the original objectives, how it has not met the
objectives, and how it could be improved.

11
3.0 Literature Review
3.1 Introduction
The likelihood of a person developing a debilitating illness, such as Parkinson’s
disease, increases with old age [1]. Parkinson’s is a degenerative neurological
disease with four primary symptoms: tremors, rigidity, Bradykinesia or Akinesia, and
postural instability [2] [3]. Tremors associated with Parkinson’s disease are a
significant struggle in the day-to-day life of those affected, resulting in the sufferer
having difficulty holding objects, as well as performing normally simple tasks. It is
therefore common for people with Parkinson’s to require assistance from either other
people or technology [4]. It is estimated that Parkinson’s disease affects 6.3 million
people worldwide, across all cultures and races, with the majority of those diagnosed
being over 60 years of age [5].
Through research and technological advancements, the medical community are
constantly striving to enhance the lives of those suffering from Parkinson’s disease,
however there is not yet a cure for the disease [6]. Recent research suggests that
adjunctive therapies can help with the neurodegenerative disease process [7].
Therapies such as dance sessions have been proven to play a significant role in
improving the health and heath-related quality of live (HRQOL) of Parkinson’s
disease (PD) patients [7].
The use of technology is ever growing in modern life, including assistive
technologies for the disabled [4]. Assistive technologies encompass anything that
may aid a disabled or disadvantaged person in performing tasks with greater ease
and confidence [8]. Although not specific to PD sufferers, current technology
assisting sufferers of PD include: easy-to-hold cutlery, grab rails, and walking sticks
[8]. The focus of this project is to enhance the comfort and usability of existing
assistive technology of which a PD sufferer can use to help them eat a meal. The
current device that is available to assist consumers when eating is, unfortunately,
very expensive [9]. This device is called “Smart” spoon and is produced by Liftware-
a Google company [10] [11]. The expense of the current device means that some
sufferers of PD are not able to afford them, and therefore must rely on various
charities to get the devices; Liftware accepts donations and will send devices to
charities for a patient on their waiting list [12].

12
This project aims to build on the existing technology to produce a “Smart” spoon of a
similar design, using an open source programmable board as the controller, and
vibration sensors and servos as input and output devices respectively. The aim is to
achieve a similar device with similar tremor stabilisation performance as the current
technology, but within a smaller budget and, therefore, at a smaller consumer cost.
This literature review will compare different methods of design and come to a
conclusion on which would be the best to base the project on.
3.2 Programmable Controllers
3.21 Programmable Controllers in Industry
Programmable controllers are versatile pieces of electronic equipment harbouring
many applications and a wide market. The commonly used programmable controller
is formally known as the Programmable Logic Controller (PLC). PLC’s are almost a
necessary piece of equipment in any industry, having replaced hardwired
electromechanical devices, since being introduced in 1969 [13]. When first
introduced into industry PLC’s soon became extremely popular within industry and
personal use; this demand for programming equipment lead PLC manufactures to
enable a way to program the users equipment using personal computers and thus
resulted in further market growth of the devices and simplification of the
programming language so that not only programming experts are able to program
them [13] [14]. The PLC has played a significant part within the industrial revolution;
primarily focused in control system engineering [14], enabling more complex
systems to be installed in significantly smaller housings, relative to the area required
to install an electromechanical system.
The following paragraph is sourced from [14]. A PLC is a type of microprocessor-
based controller consisting of a programmable memory to store and implement
various functions such as, timing, sequences and calculations, resulting in the
operation of machinery. PLC versatility means that one, or many, controllers can
easily be linked to enable the user to have as many inputs and outputs as required.
The hardware for a basic PLC usually consists of a processor, power supply,
memory, inputs and outputs; figure 1 shows how this could be arranged as a block
diagram.

13
Data is transferred using a data bus. The bus transfers data in bits, which is
interpreted as 1’s and 0’s known as binary. Binary is a digital form of communication
referring to the state of the device, usually 1 meaning on and 0 meaning off.
The industrial use of PLC’s is expected to grow due to the simplicity and versatility of
the device, however it will be far too big for the “Smart” spoon project under
consideration.
Figure 1: The PLC System [14]

14
3.22 Small Scale Open Source Programmable Controllers
Microcontrollers, simply put, are small computers that can be programmed to control
electronics [15]. Open source microcontroller projects are a growing hobby for many
enthusiasts and are a simple, practical solution to problems for engineers. The
concept of small programmable controllers was to encourage more people to
become involved with computer science and programming languages [16]. The
increase in processor speed and reduction in cost of technology over the last decade
has meant that technology has been available at a low cost, therefore enabling the
average person to afford new technology [17].
The “Smart Spoon’ project will require both, a fast and small, controller. The
availability of such devices is easy to acquire; Gordon Moore’s, cofounder of Intel,
prediction states that processor speed doubles every 18 months [18] due to smaller
transistors, resulting in more being fitted onto a microprocessor and therefore
increasing the processing speed [19].
Two programmable controllers stand out from the rest as the easiest to learn and
most flexible in terms of their applications, Arduino and Raspberry Pi. Further
research will be conducted throughout this literature review to decide which
microcontroller will be best suited for the “smart” spoon application.
Figure 2: Decline in cost of Information Technology from 1960-2015 [17]

15
3.23 Arduino
The following information in section 2.3 is referenced from [15], unless otherwise
stated.
Arduino is a microcontroller for beginners with an open-source sharing platform for
developers and Arduino enthusiasts to share their projects and ideas [15], “Arduino
is designed to be usable by anyone”.
Arduino is a brand name, which can refer to the hardware, Arduino board or the
Arduino software on a computer. Developed in Italy for students it was aimed to be
the price of an average meal, making it around four times cheaper than the next
similar circuit board in 2006. The Arduino board under consideration is the Arduino
Uno R3, due to it being a suitable board for beginners and professionals. There are
much smaller Arduino boards, which may be used for the “Smart” spoon project,
however, for the prototyping process the Arduino Uno is very practical; it is large and
hence easy to work with, has many interfaces and is cheap.
The functionality of the Arduino Uno is based around the ATmega328 microcontroller
chip, made by Atmel, and known as an IC (Integrated Circuit). The ATmega328 is
available in different forms, known as packages; Plated Through Hole (PTH), and
Surface Mount Device (SMD) are two of the packages found on the Arduino Uno,
which refer to how the chip is placed on the circuit board. External devices can be
connected to the Arduino Uno as inputs or outputs via header sockets, labelled as
Digital Pins, Analog Pins and Power Pins.
Figure 3: Arduino Uno Diagram [15]

16
The digital pins on the Arduino Uno can be used to send or receive signals as either
1 or 0, on or off states. A digital reading in the case of the Arduino Uno would be 0V
or 5V and nothing in-between. An example of a digital input would be the output of a
push button being, either, on or off.
Analog pins of the Arduino Uno enables the microcontroller to receive signals at any
point between 0V and 5V. An example of an analog input, in relation the “Smart”
spoon project, could be the output from an accelerometer. The Arduino is unable to
give analog output signals. Instead Arduino is able to “mimic” the effect of an analog
output by using Pulse Width Modulation (PWM), which is a digital output.
The USB socket on the Arduino Uno is the interface used to connect the Arduino
board to a computer for programming or analysis. Programming must be written in C
language for the Arduino Uno to understand, there are many different programs
available; however the Arduino software is the recommended program for Arduino
programming. Other programming software is available which uses building blocks;
this software can be useful for beginners or people who learn better visually. Visual
programming software includes ArduBlock and Snap4Arduino [20]. Enabling Arduino
to be programming using different programming languages, such as Python, is
possible through a serial input; available software is pySerial. Python programming
language is often used by more advanced users, but is often used to move onto
higher forms of programming [20].

17
Processing speeds are often an important part when selecting a microcontroller. If
processing speeds are not sufficient then the program will not run smoothly resulting
in poor hardware performance. The Arduino Uno specifications are shown in the
table below [21].
Figure 4: Table of Arduino Uno Specifications [21]
The Arduino Uno has sufficient inputs, outputs and processing speeds for the
“Smart” spoon project and will provide an easy, yet suitable, working environment so
that the task should not only be accessible by programming experts.
3.24 Raspberry Pi
The following information in section 2.4 is referenced from [22], unless otherwise
stated.
Raspberry Pi, much like Arduino, is a credit card sized computer for an inexpensive
price; making it ideal for experimenting and learning with. Raspberry Pi has been
developed to encourage children to learn programming languages, explore
electronics and get involved with the technical side of computing. However it is not
only for children, Raspberry Pi’s applications make it ideal for advanced users to
take advantage of the abilities of the microcontroller; use it as a web browser, spread
sheet processor or electronic circuit controller.
Raspberry Pi can be thought of as a small computer rather than a microcontroller.
Arduino is a simple microcontroller for mainly small projects, whereas Raspberry Pi
has much more processing power and therefore provides a much wider range of
applications. Raspberry Pi has HDMI and video composite outputs, which enables
the device to give visual outputs onto televisions or other monitors. The underlining
operating system of the Raspberry Pi is Linux, however due to the flexibility of the
Name Processor Operating/
Input
Voltage
CPU
Speed
Analog
In/ Out
Digital IO/
PWM
EEPRO
M [KB]
SRAM
[KB]
Flash
[KB]
USB UART
Arduino
Uno
ATmega3
28P
5V/7-12V 16MHz 6/0 14/6 1 2 32 Regular 1

18
microcontroller it is possible to write your own operating system and start up
programs for the device.
The circuitry of the Raspberry Pi is built around one main processor IC, as with the
Arduino. The Raspberry Pi runs a 32 bit, 700 MHz System on a Chip, which is built
on the ARM11 architecture. The rest of the circuit board is a series of interface
ports, inputs and outputs:
• SD (Secure Digital) Card port: for storing data and installing operating
systems.
• USB ports: Used for connecting to external devices and supplying power.
• Ethernet port: Enables network connection.
• HDMI connector: Gives video and audio output to an external monitor.
• LED indicators: Shows the status of various processes of the Raspberry Pi.
• Analogue audio output: A 3.5mm jack to supply audio to various sound
outputs such a headphones and speakers.
• Composite video output: A standard RCA jack to provide a video output.
• Power input: Supplied via a micro USB port
• General purpose input and output and other pins: Used to connect to various
devices such as LED’s and buttons.
• Display serial interface (DSI) connector: can be used with a ribbon cable to
connect to an external display.
• Camera serial interface (CSI) connector: Allows a camera module to be
connected to the board.
Figure 5: Raspberry Pi Labelled Diagram [23]

19
Raspberry Pi has many more applications and greater versatility than Arduino which
makes it ideal for more technical projects, however, the “Smart” spoon project does
not require such a level of complexity and therefore the Raspberry Pi would,
potentially, be excessive for this project, however it will still be considered. When
evaluating what requirements the “smart” spoon will require, the layout of operation
is not overly complex and can be represented in a simple flow diagram.
Figure 6: Flow Diagram of “Smart” Spoon operation
Note that the flow diagram is an extremely simplified version of the operation of the
“Smart” spoon. In terms of microcontroller specifications, the microcontroller will
need to be able to handle a single input from a sensor, be able to process the
information via a specified set of instructions (the code), and then write the output to
the servo motor. For these reasons, the Raspberry Pi would be excessive and
therefore the Arduino maybe more suitable for the task.

START
(Input from sensor)
PROCESSING
(Is movement
voluntary or
involuntary?)
OUTPUT
(Move servomotor to
required position to

20
3.3 Sensors and Transducers
The inputs and outputs to a microcontroller, as discussed in section 2, must have
some type of signal to act as that input or output. The “Smart” spoon must detect the
tremors associated with a Parkinson’s disease sufferer; this requires a sensor and
transducer.
The following information is in reference to [24].
A transducer, in terms of electronics, is a device, which converts a measurable
physical quantity into an electrical signal that can be measured, such as
acceleration, strain and mass. The measurable non-electrical physical quantity is
known as the measurand. The transducer converting the physical input could be
called an input transducer; nevertheless there are also transducers used to convert
one electrical signal to another to give an output, these are known as output
transducers. An example of a system that uses input and output transducers is a
microphone amplification system; the sound is converted to an electrical signal,
amplified, then converted back to a sound at a greater volume.
Sensors are often known as transducers; they measure and respond to the
measurand. Sensors, which operate entirely from the input signal, are known as
passive, whereas sensors, which require an external power supply to operate, are
known as active.
3.4 Measuring Movement
The movement of a person’s hand could be, left, right, forwards, backwards, up or
down; this variety of movement is three-dimensional and will require a suitable
device to record the movements.
Strain gauges are a common method of measuring displacement; the movement of
one point relative to another point causes a strain, the measurand, which
corresponds to a measurement of displacement [24]. Further, more advanced,
methods of measuring displacement can be performed using small electronic
components such as accelerometers, gyroscopes and magnetometers [25].

21
3.41 Accelerometers
An accelerometer is defined as “An electro-mechanical transducer used to measure
acceleration” [26]. Accelerometers come in different forms, such as piezoelectric
accelerometers, strain gauge accelerometers and a seismic mass and spring
configuration accelerometer [24].
Figure 7: Seismic mass and spring
accelerometer diagram [24].
Small electronic accelerometers are often used in devices to detect G force
associated with movement; in vehicles, high G accelerometers are used to detect
when a vehicle crashes, resulting in airbag deployment. In laptops accelerometers
are used to detect when a computer in dropped so that the hard disk can shut down
to prevent the headers crashing into the disk and damaging any data [27].
Accelerometers measure displacement, in either two or three axes [25], dependant
on the application. Two-axis accelerometers are usually sufficient for detecting
angles and movement, however if the application requires 3D positioning then a
three-axis accelerometer may need to be used [27]. The “Smart” spoon could use a
two-axis accelerometer to detect vibration and movement, however another
accelerometer, or other device such as a gyroscope or magnetometer, may have to
be used to determine the devices orientation [25]. Research is constantly on-going
discussing the possibilities of using only accelerometers for real-time pathological
tremor compensation. Feedback-controlled functional electrical stimulation (FES)
has been used to attenuate tremors, however, this functionality has been improved
by using a digital controller and accelerometers as the input sensor [28].

22
3.42 Gyroscopes
Traditional mechanical gyroscopes consist of a spinning disk on a free axis, this axis
is unaffected by any external movements and therefore maintains it’s orientation
[29]. Micro Electro Mechanical System (MEMS) gyroscopes are used in many
devices to assist in device orientation [30]. A commonly used MEMS gyroscope is
the vibration wheel gyroscope; this operates by sensing the change in distance from
a rotating wheel, on a spinning axis, and capacitive electrodes underneath the
wheel. If a rotation about any axis occurs then the distance between the spinning
wheel and capacitive electrodes will change, inducing a difference in voltage, which
is recorded and converted into a measureable figure in the X, Y and Z-axes [31]. The
“Smart” spoon may possibly need a method of measuring orientation to enable the
device not to tip and spill as the consumer moves the spoon.
Figure 8: Vibrating Wheel Gyroscope
Diagram [31]
3.43 Magnetometers
Magnetometers are devices, which can be, used to detect ferrous materials and low
magnetic fields; this attribute makes them useful in electronic devices enabling them
to be used as a compass [32]. High intensity magnetic field measuring devices are
known as guassmeters, the oldest and most common is the Hall effect guassmeter
[32]. A magnetometer could potentially be used to measure the orientation of the
“Smart” spoon device, however the downfall would be that magnetometers usually
have to be calibrated on-site before use [25].
W.T Latt [25] proposes that a real time tremor compensation device may be
constructed using only accelerometers for orientation and vibration compensation;
this is a challenging task but may be a suitable solution for the “Smart” spoon.

23
3.5 Counteracting Movement
3.51 Inputs
A closed loop system will be required to constantly receive signals and send
feedback to the input of the “Smart” spoon. The closed loop system will take
feedback from the output to “inform” the input how to adjust for the next movement.
The input components will be the sensing part of the device as discussed in section
3.0, accelerometers, gyroscopes or magnetometers.
Generally gyroscopes are used to measure low frequency signals such as drift and
orientation, whereas accelerometers are used to measure high frequency signals
such as vibrations [33].
3.52 Filtering and Processing
The input will require some processing and filtering to enable the device to function
accordingly; digital frequency filtering could achieve the desired outcome [34].
Different movements from the human body occur at different frequencies, the human
tremor lies in the band of 8-12Hz [25]. The “Smart” spoon system must be able to
determine the difference between a tremor and a voluntary movement. Low pass,
band pass and high pass filters are all ways of filtering out unwanted frequencies,
and are the most common, and therefore cheap [35]. The filters can be made of
physical circuits consisting of capacitors, resistors and inductors [35], however with
some programming skills it is possible to digitally create these filters using software
[34].
Using hardware filters increases the cost and probability of failure due to
components needing to be purchased and physical circuits being constructed, which
could cause problems. The effects of vibration can be reduced using software
functions such as notch filtering, pass filters and controversially better for reducing
mechanical vibration, input shaping [36]. Input shaping is the process of generating a
self-cancelling command signal to reduce vibration; a later portion of the command
cancels any vibration caused by a potion of the command [37].
For the “Smart” spoon to be successful in stabilising tremors it must act in real-time,
process the voluntary and involuntary movements and generate an out-of-phase

24
movement to nullify the erroneous part, all in one cycle [38]. Tremor compensation
instruments require zero-phase lag in the filtering process to enable to a
counteracted movement to be generated in real-time [38]. Algorithms developed to
overcome delay problems include:
• Weighted-Frequency Fourier Linear Combiner (WFLC).
• Band Limited Multiple Fourier Combiner (BMFLC)
There are also combinations of filters to further improve on the filtering process, such
as WFLC with Kalman filter [38]. The Kalman filter has the advantage of being able
to process a frequency input more precisely for a more accurate output, however it is
relatively complicated to use due to complex mathematics [38]. A filter, which is
simpler and potentially as effective as the Kalman filter, is the complementary filter;
this utilizes and low-pass and high-pass filter to process signals using much simpler
mathematics [39].
It is possible to experiment with, and generate, frequencies and filters using software
such as Matlab [40].

25
3.53 Outputs
The project brief states that electronic servo’s will be used to deliver the desired
output movement. A servomotor consists of a motor and an encoder; the encoder
tracks the motor’s rotation enabling the servo to turn through precise angles [15].
Most servos can usually only rotate a maximum of 180 degrees, therefore they are
useful for hobbies when constructing a moving component; often used in robotics
and RC vehicles [15].
Choosing a suitable servo is important because different servos have different
specifications, such as response time, turning angle, size, torque and voltage rating
[41]. The servo in the “Smart” spoon will require a fast response time to
accommodate the sudden, unexpected, movements of a tremor. Connecting a servo
to an Arduino board is relatively simple and straightforward to program, due to a
servo library being pre-built into the Arduino IDE software [15].
3.6 Conclusion
Parkinson’s is a disease, which causes sufferers from being able to do usually
simple everyday tasks. The aim of the “Smart” spoon project is to create a simple,
reliable and affordable device to aid people who suffer from tremors. The design and
build of the “Smart” spoon will be built of three basic structures, the input, the
processing unit and the output. The processing unit will be a microcontroller due to
the simplicity of programming and ease of circuit prototyping. Input devices will be
chosen after consideration, between accelerometers, gyroscopes and
magnetometers, or a combination of those listed. Filters must be used within the
processing unit of the “Smart” spoon to ensure the device filters out unwanted
frequencies whilst being able to detect and react to frequencies associated with
Parkinson’s tremors. The frequency filter will be chosen and fine-tuned using a trail
and error approach in simulation software. There are any possible filters to use and
combinations of filters also. An output is to be delivered via a servomotor being
controlled from a microcontroller and constantly updated using a closed loop system.
Software will be utilised to create a closed loop system using programming
techniques; using software instead of hardware will save space and avoid
unnecessary costs of components.

26
4.0 Filters
Almost every electronic system will require some form of a filter to make the circuit
respond more accurately, quicker or correctly. Many different types of filters are
available as discussed in the literature review; however, choosing the optimal filter
for an application requires analysis of what the filter will be doing.
The “smart” spoon requires a filter that will be able to make the input from the sensor
stable, smooth and able to process easily by the microcontroller. If there is too much
noise or variation in an input signal, the processor will not be able to read the input
signal accurately, therefore affecting the output of the system and the overall
performance of the device.
4.1 Average Filter
The equation for the average of a data set is the given data set divided by the
number of data in the set. If a data set contains k datum (x!, x!, … , x!), the average is
calculated by:
𝑥! =
𝑥! + 𝑥! + ⋯ + 𝑥!
𝑘
Equation 1.1: Average “Batch Expression” [42]
This expression is called the “batch expression”, this is only able to compute one set
of data in a single batch. If another datum is added to the data set, all the data will
need to be summed again and divided by 𝑘 + 1. In this case the “batch expression”
cannot be used because it is not in recursive form. Recursive expressions reuse the
previous result and hence they have good efficiency. For a large data set the
recursive form is a major advantage, thus for the numerous calculations that the
“smart” spoon will need to perform, the recursive form will be required.
The recursive form is derived from the average “batch expression” by manipulating
the formula, as shown in the following equations.

27
𝑥!!! =
𝑥! + 𝑥! + ⋯ + 𝑥!!!
𝑘 − 1
Equation 1.2 [42]
Multiply both sides of equation 1.1 by 𝑘:
𝑘𝑥! = 𝑥! + 𝑥! + ⋯ + 𝑥!
Dividing both sides by 𝑘 − 1:
𝑘
𝑘 − 1
𝑥! =
𝑥! + 𝑥! + ⋯ + 𝑥!
𝑘 − 1
The expression on the right hand side could be expressed in two terms after
separating 𝑥! from the original term:
𝑘
𝑘 − 1
𝑥! =
𝑥! + 𝑥! + ⋯ + 𝑥!
𝑘 − 1
=
𝑥! + 𝑥! + ⋯ + 𝑥!!!
𝑘 − 1
+
𝑥!
𝑘 − 1
The first term on the right hand side is identical to the definition of 𝑥!!!, as described
in equation 1.2, therefore the equation above can be written as:
𝑘
𝑘 − 1
𝑥! = 𝑥!!! +
𝑥!
𝑘 − 1
Dividing both sides by
!
!!!
, gives the recursive expression:
𝑥! =
𝑘 − 1
𝑘
𝑥!!! +
1
𝑘
𝑥!
Equation 1.3: Average Recursive Expression [42]

28
Simplifying this expression by making 𝑎 ≡
!!!
!
= 1 −
!
!
∴
!
!
= 1 − 𝛼, hence:
𝑥! =
𝑘 − 1
𝑘
𝑥!!! +
1
𝑘
𝑥!
= 𝛼𝑥!!! + (1 − 𝛼)𝑥!
Equation 1.4: Average Filter [42]
The average filter equation is important and widely used in other filters, hence the
detailed derivation in this section.
The average filter can be used for sensor initialization as well as computation of
averages. A good example of this type of application is a digital scale; the zero point
of a digital scale may change for various reasons and therefore needs to be reset
after powering on the scale. By implementing an average filter to the scale, a number
of initial measurements will be made after powering on the scale and then the new
zero point is calculated. If the scale was to use a microprocessor to store the values
and then compute the average, the price of this would be relatively expensive,
therefore an average filter is much more preferable [42].
4.2 Moving Average Filter
The moving average filter is well adapted and used in the stock market environment;
using it to see trends of prices in the stock market for such things as a 5-day moving
average curve [42]. The moving average filter is used to remove noise of a system
when the average must be taken from a set of data that is constantly moving with
time.
The moving average filter takes the average of a data set that is constantly being
updated and only the most recent data is averaged, hence the name, moving
average filter. The number of datum to average will be a constant, say 10, therefore
only the 10 most recent datum will be used for the average calculation; the oldest
datum is discarded as the data set updates.

29
The equation for the computation of the moving average of 𝑛 datum is:
𝑥! =
𝑥!!!!! + 𝑥!!!!! + ⋯ + 𝑥!
𝑛
Equation 2.1: Moving Average [42]
Note that 𝑥! for the average moving filter is the average of 𝑛 datum from 𝑘 − 𝑛 + 1 to
𝑘!!
, whereas 𝑥! for the average filter was simply the average of 𝑘 datum.
The moving average filter equation, 2.1, can be written in recursive form, the
derivation is shown below:
𝑥!!! =
𝑥!!! + 𝑥!!!!! + ⋯ + 𝑥!!!
𝑛
Equation 2.2 [42]
By comparing equation 2.1 with equation 2.2 it is shown that 𝑥! is the average value
calculated by adding 𝑥!while, at the same time, discarding 𝑥!!!.
Subtracting equation 2.2 from 2.1 gives:
𝑥! − 𝑥!!! =
𝑥!!!!! + 𝑥!!!!! + ⋯ + 𝑥!!! + 𝑥!
𝑛
−
𝑥!!! + 𝑥!!!!! + ⋯ + 𝑥!!!
𝑛
=
𝑥! − 𝑥!!!
𝑛
Leaving 𝑥! on the left hand side results in the recursive expression:
𝑥! = 𝑥!!! +
𝑥! − 𝑥!!!
𝑛
Equation 2.3: Moving average filter (Recursive form) [42]
Since the moving average filter already continuously updates the data set, and the
data set is confined to 𝑛 datum, in its standard form, it can be seen that there are no
real advantages of the moving average filter in the recursive form [42].

30
A moving average filter could potentially be used to find the average value of the
input data from a sensor continuously with respect to time, hence the “smart” spoon
could benefit from this filter. To summarize the moving average filter we can say that
the greater number of data being used to get an average, the better noise reduction
performance, but time delay is being introduced which makes variation of the data
not being projected immediately. If there are less datum in the data set then the time
delay is reduced and hence a faster computation time and output value, however,
noise reduction performance will be decreased.
4.3 Low-Pass Filter
The low pass filter is a filter that blocks high frequency signals and only allows the
low frequency signals to pass [42]. This filter is widely used in noise filtering
application because in the majority of cases the signal required is low frequency and
the noise is high frequency.
The disadvantage of using a moving average filter is that each of the data is
weighted equally, which may cause the resulting average to be relatively inaccurate
when considering a fast moving system such as a sonar system. For a sonar system
the most recent value in the data set would most likely be closest to the real value of
the position of the object, and the oldest datum would be the least accurate. For a
small data set this may not be much of a problem, however, when computing the
moving average of a large data set, the older values in the data set may be
insignificant and therefore cause error in the average value.
4.31 First Order Low-Pass Filter
It is possible to reduce the shortcomings of the moving average filter by weighting
the most recent datum heaviest, and the oldest datum lightest. The First order low-
pass filter provides the solution to this and is thought of as an estimate, instead of an
average. The recursive expression for the 1st
order low-pass filter is:
𝑥! = 𝛼𝑥!!! + (1 − 𝛼)𝑥!
Equation 3.1: 1
st
Order Low-Pass Filter (Recursive Form) [42]

31
Where 𝛼 is a constant in range of 0 < 𝛼 < 1. This equation is similar to the average
filter equation 1.4, the difference being that in equation 1.4 𝛼 was determined by the
number of datum and not arbitrarily chosen [42].
Deriving and a proving that the first order low pass filter will provide a solution,
whereby the oldest datum is weighted lightest and the most recent datum is
weighted highest, involves manipulation of equation 3.1, as shown.
The pervious estimate 𝑥!!! is:
𝑥!!! = 𝛼𝑥!!! + (1 − 𝛼)𝑥!!!
Equation 3.2 [42]
Substituting this into equation 3.1, the result is:
𝑥! = 𝛼𝑥!!! + (1 − 𝛼)𝑥!
= 𝛼 𝛼𝑥!!! + 1 − 𝛼 𝑥!!! + (1 − 𝛼)𝑥!
Equation 3.3 [42]
= 𝛼!
𝑥!!! + 𝛼 1 − 𝛼 𝑥!!! + (1 − 𝛼)𝑥!
Comparing the size of 𝛼 1 − 𝛼 and 1 − 𝛼, which are the coefficients being multiplied
by the measurements 𝑥!!! and 𝑥! respectively, and considering that 𝛼 is a constant
in the range of 0 < 𝛼 < 1, we see that the two coefficients have the following
inequality:
𝛼 1 − 𝛼 < 1 − 𝛼
Remembering this inequality and looking into equation 3.3, it is possible to see that
the most recent measurement 𝑥! has a greater weighting than the oldest
measurement 𝑥!!!, thus proving that the first order low-pass filter solves the
shortcomings of the moving average filter. The low-pass filter is useful to understand
because it occurs in the algorithm for the Kalman filter.

32
4.4 The Kalman Filter
The Kalman first was first introduced in the 1960’s and is derived from complex
mathematics, which goes beyond the scope of this project, hence for this project the
Kalman filter has been implemented from the given algorithm, instead of formulating
an entire understanding of the algorithm. The basic steps of the Kalman filter are
shown in the flow diagram below.
Figure 9: Kalman filter algorithm [42]
I. Predict state & error covariance
𝑥!!! = 𝐴𝑥!!!!
𝑃!! = 𝐴𝑃!!! 𝐴!
+ 𝜚
0. Set Initial Values
𝑥!!, 𝑃!
II. Compute Kalman Gain
𝐾! = 𝑃!! 𝐻!
(𝐻𝑃!! 𝐻!
+ 𝑅)!!
III. Compute the estimate
𝑥!! = 𝑥!!! + 𝐾!(𝑍! − 𝐻𝑥̅!! )
IV. Compute the error covariance
𝑃! = 𝑃!! − 𝐾! 𝐻𝑃!!
Estimate
𝑥!!
Measurement
𝑍!

33
In the Kalman filter algorithm the subscript 𝑘 represents that the algorithm is
recursive and for the purpose of the algorithm the subscript 𝑘 can be ignored. On the
other hand the superscript “-“ is important to notice as this means that the value is a
predicted value [42].
The steps of the Kalman filter algorithm as presented in figure 9, are as follows:
• Step I: The predicted variables used throughout the algorithm, 𝑥! and 𝑃!,
are calculated.
• Step II: In this step the Kalman gain (𝐾!) is calculated.
• Step III: An estimate is computed from a measurement given as an input.
This section of the algorithm is similar to the low-pass filter.
• Step IV: In this step the error covariance is computed. The error covariance
is a measurement of how accurate the estimated value is.
The following table is a summary of all the variables used in the Kalman filter.
External Input 𝑧!(Measurement)
Final Output 𝑥! (Estimate)
System Model Α, Η, 𝜚, 𝑅
For Internal Computation 𝑥!, 𝑃!, 𝑃!, 𝐾!
Figure 10: Table of Kalman Filter Algorithm Variables [42]
The system model variables are the variables set by the user, they are not calculated
by the Kalman filter. In the case of the smart spoon, these variables are related to
the sensor inputs and sensitivity.
The Kalman filter algorithm can be summarized in three steps:
1. Predict the state and error covariance, 𝑥! and 𝑃! respectively, for the
next time point, based on the system model variables (A and 𝜚).
2. Calculate and compensate the difference between the measurement
value and prediction value, then compute a new estimate. This new
estimate is the final result of the Kalman filter, 𝑥! and 𝑃!.
3. Loop through steps 1 and 2 continuously.
The computing of the estimate is closely related to the first order low-pass filter as
previously mentioned, this next section will briefly explain why this is the case and
derive the mathematical formulae.

34
Firstly the expression for computing an estimate is:
𝑥! = 𝑥! + 𝐾!(𝑍! − 𝐻𝑥!)
Equation 6.1: Kalman Filter estimate [42]
Where 𝑍! is the measurement and 𝑥! is the estimated value. This is sometimes
called the priori estimate [42].
Expanding and collecting terms of equation 6.1, we get:
𝑥! = 𝑥! + 𝐾!(𝑍! − 𝐻𝑥!)
= 𝑥! + 𝐾! 𝑍! − 𝐾! 𝐻𝑥!
Equation 6.2 [42]
= 𝐼 − 𝐾! 𝑥! + 𝐾! 𝑍!
Assuming H is an identity matrix I and rewriting the equation yields:
𝑥! = 𝐼 − 𝐾! 𝐻 𝑥! + 𝐾! 𝑍!
= 𝐼 − 𝐾! 𝐼 𝑥! + 𝐾! 𝑍!
Equation 6.3 [42]
= 𝐼 − 𝐾! 𝑥! + 𝐾! 𝑍!
Comparing this equation to that of a low-pass filter: 𝑥! = 𝛼𝑥!!! + (1 − 𝛼)𝑥!, and
substituting 𝛼 with 1 − 𝐾, the sorting by terms:
𝑥! = 𝛼𝑥!!! + (1 − 𝛼)𝑥!
= 1 − 𝐾 𝑥!!! + {1 − 1 − 𝐾 }𝑥!
= 1 − 𝐾 𝑥!!! + 𝐾𝑥!
Comparing this with equation 6.3 we see:
1st
order low-pass filter: 𝑥! = 1 − 𝐾 𝑥!!! + 𝐾𝑥!
Kalman Filter: 𝑥! = 𝐼 − 𝐾! 𝑥! + 𝐾! 𝑍!

35
The two equations are very similar, however, the 1st
order low-pass filter calculates
an estimate by adding weighted values of the previous estimate and measurement.
The Kalman filter works in nearly the same way, the only difference being that the
Kalman filter is not using the previous estimate; it is using a prediction [42].
The estimation process of the Kalman filter algorithm is the characteristic of the
Kalman filter that really starts to set it apart from the low-pass filter. From the flow
diagram of the Kalman filter in figure 9, the computation of the estimate is step III
and given by the equation:
𝑥! = 𝑥! + 𝐾!(𝑍! − 𝐻𝑥!)
The equation requires a prediction 𝑥! and a new measurement 𝑍! [42].
Measurement will be the input from the sensor on the “smart” spoon, the prediction is
the result of the prediction process.
The next variable to consider in the Kalman gain 𝐾!, and if the vaue of the Kalman
gain is know then it is possible to compute the new estimate. The equation for the
Kalman gain is given in step II in firgure 9 as:
𝐾! = 𝑃! 𝐻!
(𝐻𝑃! 𝐻!
+ 𝑅)!!
The mathematical derivation and proving of this equation is complex and beyond the
scope of this project, therefore for the pupose of the project it is only necessary to
understand that the Kalman gain is just being used as weighting in the process in
step III which calculates the estimate. The significance of the Kalman filter compares
to the low pass filter is that, in the Kalman filter, the weighting used for computation
of an estimate is adjusted at every step. Whereas, in the low-pass filter the weighting
is fixed.
The last step in the Kalman filter is the error covariance, as calculated in step IV by
the equation:
𝑃! = 𝑃! − 𝐾! 𝐻𝑃!.

36
The value of the error covariance is then passed on to the prediction process. The
error covariance indicates the difference between the estimate from the Kalman filter
and the true, unknown, value. Simply put the error covariance is the degree of
accuracy of the estimate.
The error covariance can be represented by the following equation:
𝑥!~𝑁(𝑥!, 𝑃!)
Equation 6.6: Error Covariance and Estimate
Meaning that 𝑥! follows a normal distribution with the mean value 𝑥! and covariance
𝑃!, therefore it can be seen that the Kalman filter computes the probability of the
estimate of variable 𝑥! and selects the one with the highest probably as the
estimate. The normal distribution curve means that the possible values that 𝑥! could
be are distribution via a bell curve, of which the width of bell curve is determined by
the value of 𝑃!. A graphical representation is shown figure 11.
Figure 11: Normal distribution of error covariance
If the width of the bell curve is small, then the estimate will be within a small range,
meaning it will be more accurate, or small. A wider bell curve results in a wider
possible range of values for 𝑥!, hence the estimate will be less accurate, or large.

37
In conclusion, the average filter cannot compute an average over time and is
therefore not suitable. The moving average filter computes the average over time for
a given data set of size 𝑛, whereby the oldest datum in the set is discarded as the
newest datum is acquired. The low-pass filter is able to filter between high and low
frequencies, allowing low frequency signals to pass, and high frequency signals are
blocked. The low-pass filter weighs each datum in the set, with the newest datum
being highly weighted and the oldest datum being low weighted. The Kalman filter
weighs the datum in a set much like the low-pass filter, however instead of using
previous states, the Kalman filter uses predictions and then compares the
predictions with measurements to compute the error covariance. The Kalman filter,
therefore, is better described to be an estimation algorithm rather than a filter.
Previous research, as discussed in the literature review, has more evidence
supporting the advantages of using the Kalman filter compared to similar filters,
hence the Kalman filter is the most widely used filter for sensor fusion [42]. However
there is one last filter to discuss, this is the complimentary filter.
4.5 Complimentary Filter
The complimentary filter is similar to the Kalman filter in terms of being able to
combine sensor readings, known as sensor fusion [42]. The complimentary is often
considered to have a greater advantage over the Kalman filter when processing
frequency signals. The reason for this is that the complimentary filter is essentially a
combination of the low-pass and the high-pass filter. For an application where the
sensors in question are needing to be combined are complimentary to each other,
for example one sensor is sensitive to low frequency signals and the other is
sensitive to high frequency signals, the complimentary filter may be used [42].
The transfer function block diagram in figure 12 shows the function of complimentary
filter when combining two signals from two sensors to achieve an overall
measurement.

38
Figure 12: Complimentary Filter Block Diagram [42]
Here, 𝑥 𝑡 is the true value of the physical quantity of interest, 𝑛!(𝑡) and 𝑛! 𝑡 are
the noise included in the measurement from each sensor, 𝐺!(𝑠) and 𝐺!(𝑠) are the
filters processing the measurement from each sensor, and 𝑧(𝑡) is the final result of
the measurement [42].
By blending the measurements from each of the sensors the true original value can
be found, after the filters have applied the appropriate processing. This is achieved
by carefully designing the filters 𝐺!(𝑠) and 𝐺!(𝑠) for the appropriate noise
characteristic of each signal [42].
𝐺!(𝑠)
𝐺!(𝑠)
𝑧(𝑡) ≈ 𝑥(𝑡)
+
+
𝑥(𝑡) + 𝑛!(𝑡)
𝑥(𝑡) + 𝑛!(𝑡)

39
5.0 Components
The components used in this project have been selected to fulfil the objective of
developing a low cost alternative to the Liftware labs “smart” spoon, with this
objective in mind the components, therefore, must be easily available and mass
produced to reduce costs. The project brief specifies, “The system will be based on a
microcontroller board, vibration sensors and servo motors to counteract the spoon’s
vibrations” [43]; limiting the range of components able to use.
5.1 Microcontroller
As discussed in the literature review there are several options for which
microcontroller to use, the main two being the Raspberry Pi and the Arduino brands.
After comparing the specifications of each board it was clear to see that not all of the
ports and options would be made use of when using a Raspberry Pi, therefore using
the cheaper and simpler Arduino board would be more suitable. Another advantage
of using the Arduino is that the programming language is based around C
programming, a commonly used programming language. Comparing this to the
Raspberry Pi programming language, python, it seems using the Arduino is the more
suitable option for myself.
5.11 Arduino Nano
The Arduino board has many variations of the board including the Arduino Uno, as
discussed in the literature review, and the Arduino Nano. The Arduino Nano is based
on the ATmega328 microprocessor chip, meaning it has the same processing power
as the Arduino Uno in a much smaller package [44]. The Nano can be powered
either by a micro USB cable or by an external power supply between 6V and 20V
(Maximum limits), therefore is ideal for a battery-operated device [44]. The size of
the Arduino Nano is the most important factor when considering that the
microcontroller must be able to fit within the “smart” spoon handle comfortably. The
size of the Arduino Nano is 45mm in length, 18mm in width and weighs 5g, making it
a suitable choice for the main controller of the “smart” spoon.

40
Figure 13: Arduino Nano Front [44] Figure 14: Arduino Nano Back [44]
Comparing the Arduino Uno and the Arduino Nano we can see that the performance
specifications are very similar.
Figure 15: Arduino Board Comparison [21]
The size and performance of the Arduino Nano, makes it a suitable microcontroller
for the “smart” spoon, however due to the easy accessibility of the Arduino Uno’s
large size and greater number or pins, the Arduino Uno’s is better suited for
breadboard prototyping, initial setup and testing.
The pin diagram for the Arduino Nano is shown in figure 16.
Figure 16: Arduino Nano Pin Diagram [45]

41
5.2 Input Sensor
As discussed in the literature review there are many types of sensors for detecting
movement and vibrations, the main ones being accelerometers and gyroscopes. A
combination of the two on a single chip is commonly known as an IMU (Inertial
Movement Unit). An IMU is a very common electronic component, used in
smartphones, tablets, and various devices, which require a measurement of
orientation or movement [46].
5.21 MPU-6050
The MPU-6050 is an IMU, part of the MPU-60X0 series of IMU’s produced by
Invensense. The MPU-6050 combines a three-axis accelerometer and a 3-axis
gyroscope, to produce a 6 DOF sensing device. The unit operates as an I2
C device,
meaning that communication to the Arduino, or other device, is sent through only two
wires, the serial clock line (SCL) and the serial data line (SCL), also known as the
serial BUS. Other devices may have individual wires or connections for each axis of
the gyroscope and the accelerometer, which increases the space required for
implementation, complexity and cost. An I2
C device has the advantages of being
easily integrated into existing circuitry and having few pins or wires, thus reducing
space required for implementation and also reducing cost [47]. Examples of I2
C
devices are USB devices. A USB connector to a computer will usually only have four
pins; ground, positive voltage, SCL and SDL.
Figure 17: MPU-6050 [48]
I2
C communication occurs between a master and a slave, the master controls a
slave one at a time. The communication is sent it bits: a simplified description of I2
C
communication is:
• Master sends a START condition to a slave.
• The master then addresses the required slave using a 7-bit address.

42
• The 8th
bit after the START condition tells the slave whether it is to receive or
send data.
• The slave must then acknowledge the master and start data transfer.
• The slave must acknowledge after every 8th
bit has been transferred.
• Once all the data has been sent or received, the master issues and STOP
condition, freeing the slave [49].
The cost, availability and popularity of the MPU-6050 make this IMU ideal for the
“smart” spoon project. The average cost of the device £1.50 to £3.50 and is usually
available online for fast delivery [50]. The popularity of the MPU-6050 means there
are many resources to learn about and ask questions about the device; websites
such as Github and the Arduino playground forum provide many educational
resources for beginners.
For the “smart” spoon, the MPU-6050 IMU can be easily connector to the Arduino
Uno and, in turn, the Arduino Nano, as shown in figure 18.
Figure 18: Arduino Uno MPU-6050 Wiring [51]
The connections of the MPU-6050 that are used with the Arduino are as follows:
• MPU-6050, VCC – Arduino, 3.3V (Diagram shows 5V, this for other models
of the MPU-6050)
• MPU-6050, GND – Arduino, Ground
• MPU-6050, SCL – Arduino, A5 (Analog pin 5)
• MPU-6050, SDL – Arduino, A4 (Analog pin 4)
• MPU-6050, INT – Arduino, Pin 2 (interrupt pin)

43
Analogue pins 4 and 5 on the Arduino Uno are also the SDL and SDL connections
for the board. The digital interrupt, pin 2, allows the MPU-6050 to interrupt the
current process of the Arduino for a fraction of time to compute something else, this
happens asynchronously, after this the Arduino is free to continue running the code.
5.3 Servo Motor
The project brief specifies that a servomotor must be used in the “smart” spoon to
counteract the tremors of the users hand. To create a real time tremor compensation
device, the servo must be able to react in a precise and fast manor. Another attribute
of the servomotor is that it must have smooth operation to ensure there are no
judders in the output. To be able to be integrated within the “smart” spoon and hence
inside a users hand, the servomotor must be small and light. With these
specifications in mind, the following servomotors have been taken into consideration.
5.31 SM-S2309S Servomotor
The SM-S2309S is a small, analog, lightweight and low voltage servomotor designed
for hobbyists. The hobby servo is ideal for use in remote control devices such as
planes, boats and multi-rotor copters. The specifications of the SM-S2309S are:
Figure 19: SM-S2309S Specifications [52]

44
The servomotor will be supplied by the 5V output pin from the Arduino, thus being
able to provide approximately 1.1kg*cm of torque and a speed of 60°/sec, making it
suitable for the “smart” spoon; assumptions made based on the average weight a
spoon, plus the weight of food on the spoon being significantly less that 1.1kg.
The servo is connected via a 3-pin servo connector consisting of three wires,
positive, ground and the signal wire. Connections to the Arduino are very simple, the
positive and ground connections will be made by connecting to the corresponding
pins on the Arduino, the signal wire can be connected to any digital output pin on the
Arduino; the pin diagrams of the Arduino Uno and the Arduino Nano are shown in
figures 3 and 16 respectively.
5.32 HKSCM9 Servomotor
The HobbyKing SCM9 is a 5V hobby servomotor similar to the HM-S2903S
servomotor. They have similar torque output, weight, size and operating voltage. A
table of specification is shown in figure 20.
Figure 20: HKSCM9 Specifications [53]
Comparing these two servomotors, we see that the difference between them is
negligible.

45
6.0 Building the “Smart” Spoon
This section of the project report aims to explain how, and which, of all of the
components and filters, previously discussed, have been implemented into the
development of the “smart” spoon. After this section the reader should be able to
understand methods used and why they have been chosen, and be able to relate the
elements of the “smart” spoon back to the principles and methods discussed
previously in the report and the literature review.
6.1 Setting up
In the initial stages of the project it was clear that there would need to be many test
circuits built on breadboard, thus due to the larger and more easily accessible pins
on the Arduino Uno, this Arduino was used for much of the testing. By using the
tutorials and circuit diagrams given on the Arduino Playground, learning to
understand how to use the MPU-6050 with the Arduino was possible.
To test the functionality of the MPU-6050 and understand how to program the
Arduino to “talk” to the MPU-6050, it was necessary to firstly set up the simple circuit
as shown in figure 21 below.
Figure 21: Arduino Uno – MPU-6050 setup
Using this set up and the codes given in the Arduino playground, it was possible to
use the serial monitor in the Arduino IDE to see the raw values from the MPU-6050.
The term raw values are used because the MPU-6050 automatically applies filters to
the gyroscope and the accelerometer to get a filter output; by reverse engineering
the filters, we are able to get raw data from the gyroscope and accelerometer [48].

46
The code for the raw values is shown in figure 22.
Figure 22: Code for Raw Data Values [54]
This basic code is important to understand because after this, the code gets more
complex. The next few paragraphs of this report will go through some of the basic
bits of code in order.
The first line of the code “#include<Wire.h>” tells the Arduino IDE to include the wire
library to enable specific commands to be used. A library is basically a set of
instructions that can be accessed and used by hardware when specified. The Wire
library includes all the commands to enable data to be sent and received to and from
I2
C devices, via the SDL and SCL [55].
The following line of code defines the constant integer “MPU_addr” as the address of
the MPU-6050, which is 0x68, “const int MPU_addr=0x68;”. The addressing of the
MPU-6050 is a register address, found in the MPU-6050 register map and
descriptions document. The register documents shows that the address “0x68”,

47
which is register 68 (hexadecimal), is the address used to reset the analog and
digital signal paths of the gyroscope, accelerometer, and temperature sensors.
Resetting the sensors reverts the signal path, analog to digital converters, and filters
to their power up configurations. The reset also initializes the serial interface [56].
Some of the registers used for the MPU-6050 are shown in figure 23.
Figure 23: MPU-6050 Data Registers [56]
Each axis of the accelerometer and gyroscope has two registers, an internal register
set and a user facing read register set. The data within the sensors’ internal register
set is always updated at the Sample Rate. Meanwhile, the user facing read register
set duplicates the internal register set’s data values whenever the serial interface is
idle. This guarantees that a burst read of sensor registers will read measurements
from the same sampling instant.
The following line of code “int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;” defines the
variables as 16-bit integers. These are outside of the loop and setup and are
therefore referred to as global variables. So far, the lines of code shown in figure 24
have been explained.
Figure 24: Global Variables
In the setup part of the code, the serial wire transmission between the Arduino and
the MPU-6050 the sensor values are reset, the MPU-6050 is “woken up” and is
initiated at a baud rate of 9600; set by the line of code “Serial.begin(9600);”. The

48
baud rate is simply the speed at which bits are sent along the transmission wire. The
setup section of the code is shown in figure 25.
Figure 25: Setup Definitions
The loop part of the code is the section, which runs continuously. In this section the
Arduino requests the required data from the registers and prints that data into the
serial monitor to enable the user to see the data. The loop starts by resetting and
waking up the MPU-6050, then requesting the registers 3B to 48, shown in figure 23.
Proceeding, the code then prints the values of the global variables, these are the
MPU-6050’s accelerometer X, Y and Z-axes values, the gyroscope X, Y and Z-axes
values and the temperature. Following this section of code there is a delay of 100
milliseconds (ms); the delay increases stability of the loop. The loop section is shown
in figure 26.
Figure 26: MPU-6050 Arduino Loop
These basic coding methods of communicating with I2
C device registers will be
extensively used in the application of the Kalman filter and the “smart” spoon.

49
6.2 The Kalman Filter Code
The Kalman filter is used in many devices and research has proven that it is an
efficient and relatively accurate way to blend two or more sensor measurements [42];
therefore the Kalman filter will be suitable for the “smart” spoon.
The Kalman filter will use a series of measurements taken over a period of time; in
this case these are measurements from the gyroscope and accelerometer. The
measurements will contain noise, which will cause error in the system. The Kalman
filter will then estimate the state of the system based on the current and previous
states, therefore making a more precise estimation [57]. In short, the reason for
using the Kalman filter is that the accelerometer is very noisy and therefore cannot
by relied upon over a short time, whereas the gyroscope tends to drift over time and
therefore cannot be trusted over long periods of time [57].
The Kalman filter code for Arduino is available via online resources, which have
been thoroughly analysed to ensure a complete understanding. The main sections of
the code used for I2
C communication to the MPU-6050 and the Arduino library used
have been acquired from the Arduino website and Github. The implementation of the
Kalman filter is used from a practical tutorial written by Kristian Lauszus [57].
Some of the notations and variables differ from the Kalman filter previously
described, however they are not hard to follow and follow the same steps.
6.21 The System State 𝒙 𝒌
The first part of the Kalman filter is to estimate the state of the system, given by:
𝑥! = 𝐹𝑥!!! + 𝐵𝑢! + 𝑤!
Equation 7: System State [57]
Where 𝑥! is the state matrix given by:
𝑥! =
𝜃
𝜃! !
Equation 7.1: State Matrix [57]

50
We see that the output of the filter is the angle 𝜃 and also the bias 𝜃!, which are
based upon measurements from the accelerometer and gyroscope; the bias is the
amount the gyroscope has drifted. The true rate can be calculated by subtracting the
bias from the gyroscope measurement [57].
Next we have the 𝐹 matrix. This is the state transition model, which is applied to the
previous state 𝑥!!!, defined as:
𝐹 =
1 −∆𝑡
0 1
Equation 7.3: State Transition Matrix [57]
Further on in the system state equation we have 𝐵𝑢!, where 𝑢! is the control input.
In this case 𝑢! is the gyroscope measurement in degrees per second at time k and
therefore known as the rate 𝜃. Hence the sate equation becomes:
𝑥! = 𝐹𝑥!!! + 𝐵𝜃! + 𝑤!
Equation 7.4: System State with rate [57]
Next is the 𝐵 matrix, known as the control input model, defined as:
𝐵 =
∆𝑡
0
Equation 7.5: Control Input Matrix [57]
Multiplying the rate 𝜃 by the delta time ∆𝑡 will give the angle 𝜃; therefore the matrix
multiplication makes sense.
Next we have 𝑤!, this is the process noise, which is Gaussian distributed with a zero
mean and with covariance 𝜚 to the time k:
𝑤!~𝑁(0, 𝜚!)
Equation 7.6: Process Noise Equation [57]
Here, 𝜚! is the process noise covariance, which in this case, is the covariance matrix
of the state estimate of the accelerometer, and the bias. The estimate of the bias and
the accelerometer is considered to be independent for this case and therefore it is
equal to the variance of the estimate of the accelerometer and bias [57].

51
The final matrix is:
𝑄! =
𝑄! 0
0 𝑄!!
∆𝑡
Equation 7.7: Covariance Matrix [57]
The 𝑄! covariance matric depends on the current time k, so the accelerometer
variance 𝑄! and the variance of the bias 𝑄!!
is multiplied by the change in time ∆𝑡.
Considering this we see that this makes sense since the process noise will be larger
as the time increases between the update of the state; for example this may be the
drifting of the gyroscope [57]. If the value of the accelerometer variance is large, the
more noise in the estimation of the state. For example, if the estimated angle starts
to drift then the value of 𝑄!!
needs to be increased. Otherwise if the estimate is too
slow then the system is relying too much on the on the estimate of the angle and
therefore the value of 𝑄! should be decreased to make the system more responsive
[57].
6.22 The measurement 𝒛 𝒌
The measurement 𝑧! is an observation or measurement of the true state 𝑥!. The
observation 𝑧! is given by:
𝑧! = 𝐻𝑥! + 𝑣!
Equation 7.8: Observation of true state [57]
The equation shows that the observation 𝑧! is given by the current state 𝑥! multiplied
by the 𝐻 matrix plus the measurement of the noise 𝑣!. 𝐻 is known as the
observation model and is used to map the true space into the observed space. We
are not able to observe the true space. Since the measurement is the measurement
given by the accelerometer, H is given by:
𝐻 = 1 0
The noise of the measurement is Gaussian distributer with a zero mean and 𝑅 as the
covariance:
𝑣!~𝑁(0, 𝑅)

52
Where 𝑅 is not a matrix and therefore the measurement noise is equal to the
variance of the measurement, since the covariance of the same variable is equal to
the variance [57]. 𝑅 is defined as:
𝑅 = 𝐸 𝑣! 𝑣!
! = 𝑣𝑎𝑟(𝑣!)
Equation 7.9: R Equation [57]
Assuming that the measurement noise is the same and does not depend on time k.
𝑣𝑎𝑟 𝑣! = 𝑣𝑎𝑟(𝑣)
If the measurement noise variance 𝑣𝑎𝑟(𝑣) is set too high, the filter will respond very
slowly as it is less trusting of the new measurements. On the other hand, if the value
is too small then the filter may overshoot the value and be noisy since it will trust the
new measurements too much [57].
6.23 Prediction
As discussed in the filters section of this report, the Kalman filter attempts to predict
the current state and the error covariance matrix at time k. First the filter tries to
estimate the current state based on all previous states and the gyroscope
measurement, equation 8:
𝑥!|!!! = 𝐹𝑥!!!|!!! + 𝐵𝜃!
Equation 8: Control Input [57]
This is also called the control input because it is used as an extra input to estimate
the state at the current time k, called the priori state. The priori state is the estimate
of the state matrix at the current time k based on the previous state of the last
system and the estimates of the states before it [57].
The next stage in the Kalman filter is to be able to predict the error covariance matrix
𝑃!|!!! based on the previous error covariance matrix 𝑃!!!|!!!, defined as:
𝑃!|!!! = 𝐹𝑃!!!|!!! 𝐹!
+ 𝑄!
Equation 8.1: Error Covariance Equation [57]

53
This matrix is used to estimate the difference between the true values and the
estimated values, known as the error, as discussed in the Kalman filter section
earlier in the report. The error covariance matrix is:
𝑃 =
𝑃!! 𝑃!"
𝑃!" 𝑃!!
Equation 8.2: Error Covariance Equation [57]
6.24 Update the System
The Kalman filter measurements must be constantly updated to provide the best
possible estimations and outputs as discussed earlier in the report. The first step is
to compute the difference between the measurement 𝑧! and the priori state 𝑥!|!!!,
this is also known as the innovation:
𝒴! = 𝑧! − 𝐻𝑥!|!!!
Equation 8.3: Innovation Equation [57]
The observation matrix 𝐻 is used to map the priori state 𝑥!|!!! into the observed
space, which is the measurement from the accelerometer, meaning that the
innovation is not a matrix and is given by:
𝒴! = 𝒴 !
Equation 8.4: Innovation Equation 2 [57]
Next is the computation of the innovation covariance:
𝑆! = 𝐻𝑃!|!!! 𝐻!
+ 𝑅
Equation 8.5: Innovation Covariance [57]
The innovation covariance predicts how reliable the measurement is, based on the
priori error covariance matrix 𝑃!|!!! and the measurement covariance matrix 𝑅. The
observation model 𝐻 is used to map the priori error covariance matrix 𝑃!|!!! into
observed space [57].

54
As the value of the measurement noise increases, so does the value of 𝑆, meaning
that the incoming measurement is not as reliable. In this case 𝑆 is not a matrix and
can be written as:
𝑆! = 𝑆 !
6.25 The Kalman Gain
The Kalman gain was briefly explained earlier in the report; however, this section
aims to explain it in more detail, which will progress to shown how to Kalman gain is
written in C language for Arduino. For the purpose of implementing the Kalman filter,
the Kalman gain can be said as being used to indicate how reliable to innovation is.
It is defined as:
𝐾! = 𝑃!|!!! 𝐻!
𝑆!
!!
Equation 8.6: The Kalman Gain [57]
The Kalman gain can be written as a 2 x 1 matrix:
𝐾 =
𝐾!
𝐾!
Equation 8.7: The Kalman Gain Matrix [57]
Now the posteriori estimate of the current state can be updated:
𝑥!|! = 𝑥!|!!! + 𝐾! 𝒴!
Equation 8.8: Posteriori Estimate of Current State [57]
The posteriori state is the estimate of the state at time k, given the observations up
to and including time k [57]. The posteriori state is calculated by adding the priori
state 𝑥!|!!! to the Kalman gain, multiplied by the innovation 𝒴!. The innovation is the
difference between the measurement 𝑧!and the estimated priori state 𝑥!|!!!,
therefore the innovation can be positive or negative [57]. This basically means that
the estimate of priori state 𝑥!|!!!, that was calculated using the previous state and
the gyroscope measurement, is corrected with the actual measurement; in this case
this is the accelerometer measurement.

55
The final step is to update the posteriori error covariance matrix:
𝑃!|! = (𝐼 − 𝐾! 𝐻)𝑃!|!!!
Equation 8.9: Update Posteriori Error Covariance Matrix [57]
Where 𝐼 is the identity matrix and is defined as:
𝐼 =
1 0
0 1
6.3 Implementing the Filter
This section of the report will explain how all of the above equations are being
implemented in C programming language. The structure of this section continues to
closely follow the “Practical appoach to Kalman filter and how to implement it” guide
by Kristian Lauszus, as published on TKJ Electronics, reference [57].
Step 1 - The control input 𝒙 𝒌|𝒌!𝟏:
𝑥!|!!! = 𝐹𝑥!!!|!!! + 𝐵𝜃!
Putting this equation in terms of the matrices discussed throughout the previous
sections, and simplifying by using simple matrix mathematics.
𝜃
𝜃! !|!!!
=
1 −∆𝑡
0 1
𝜃
𝜃! !!!|!!!
+
∆𝑡
0
𝜃!
=
𝜃 − 𝜃!∆𝑡
𝜃! !!!|!!!
+
∆𝑡
0
𝜃!
=
𝜃 − 𝜃!∆𝑡 + 𝜃∆𝑡
𝜃!
=
𝜃 + ∆𝑡(𝜃 − 𝜃!)
𝜃!
Equation 9: Control Input Matrix [57]

56
This shows that the priori estimate of the angle 𝜃!|!!! is equal to the estimate of the
previous state 𝜃!!!|!!! plus the unbiased rate, multiplied by the change in time ∆𝑡.
This can be written in C as:
rate = newRate - bias;
angle += dt * rate;
Step 2 – Predict the Error Covariance: 𝑷 𝒌|𝒌!𝟏
𝑃!|!!! = 𝐹𝑃!!!|!!! 𝐹!
+ 𝑄!
Writing this equation in matrix from and simplifying again we obtain:
𝑃 =
𝑃!! 𝑃!"
𝑃!" 𝑃!! !|!!!
=
1 −∆𝑡
0 1
𝑃!! 𝑃!"
𝑃!" 𝑃!! !!!|!!!
1 0
−∆𝑡 1
+
𝑄! 0
0 𝑄!!
∆𝑡
=
𝑃!! − ∆𝑡𝑃!" 𝑃!" − ∆𝑡𝑃!!
𝑃!" 𝑃!! !!!|!!!
1 0
−∆𝑡 1
+
𝑄! 0
0 𝑄!!
∆𝑡
=
𝑃!! − ∆𝑡𝑃!" − ∆𝑡(𝑃!" − ∆𝑡𝑃!!) 𝑃!" − ∆𝑡𝑃!!
𝑃!" 𝑃!! !!!|!!!
1 0
−∆𝑡 1
+
𝑄! 0
0 𝑄!!
∆𝑡
=
𝑃!! − ∆𝑡𝑃!" − ∆𝑡 𝑃!" − ∆𝑡𝑃!! + 𝑄!∆𝑡 𝑃!" − ∆𝑡𝑃!!
𝑃!" − ∆𝑡𝑃!! 𝑃!! + 𝑄!!
∆𝑡
=
𝑃!! + ∆𝑡(∆𝑡𝑃!! − 𝑃!" − 𝑃!" + 𝑄!) 𝑃!" − ∆𝑡𝑃!!
𝑃!" − ∆𝑡𝑃!! 𝑃!! + 𝑄!!
∆𝑡
These equations can be written C programming language as:
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;

57
Step 3 – Updating the system (Innovation): 𝓨 𝒌
𝒴! = 𝑧! − 𝐻𝑥!|!!!
= 𝑧! − 1 0
𝜃
𝜃! !|!!!
= 𝑧! − 𝜃!|!!!
The innovation is now calculated in C as:
float y = newAngle - angle;
Step 4 – Innovation Covariance: 𝑺 𝒌
𝑆! = 𝐻𝑃!|!!! 𝐻!
+ 𝑅
= 1 0
𝑃!! 𝑃!"
𝑃!" 𝑃!! !|!!!
1
0
+ 𝑅
= 𝑃!!!|!!!
+ 𝑅
= 𝑃!!!|!!!
+ 𝑣𝑎𝑟(𝑣)
The equation is written in C langue as follows:
float S = P[0][0] + R_measure;
Step 5: The Kalman Gain: 𝑲 𝒌
𝐾! = 𝑃!|!!! 𝐻!
𝑆!
!!

58
𝐾!
𝐾! !
=
𝑃!! 𝑃!"
𝑃!" 𝑃!! !|!!!
1
0
𝑆!
!!
=
𝑃!!
𝑃!" !|!!!
𝑆!
!!
=
𝑃!!
𝑃!" !|!!!
𝑆!
The equation written in C programming language is:
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
Step 6: Update the Posteriori Estimate of the Current State
𝑥!|! = 𝑥!|!!! + 𝐾! 𝒴!
𝜃
𝜃! !|!
=
𝜃
𝜃! !|!!!
+
𝐾!
𝐾! !
𝒴!
=
𝜃
𝜃! !|!!!
+
𝐾! 𝒴
𝐾! 𝒴 !
Written in C programming language the code is:
angle += K[0] * y;
bias += K[1] * y;
Step 7 – Update the Posteriori Error Covariance Matrix
𝑃!|! = (𝐼 − 𝐾! 𝐻)𝑃!|!!!

59
𝑃!! 𝑃!"
𝑃!" 𝑃!! !|!
=
1 0
0 1
−
𝐾!
𝐾! !
1 0
𝑃!! 𝑃!"
𝑃!" 𝑃!! !|!!!
=
1 0
0 1
−
𝐾! 0
𝐾! 0 !
𝑃!! 𝑃!"
𝑃!" 𝑃!! !|!!!
=
𝑃!! 𝑃!"
𝑃!" 𝑃!! !|!!!
−
𝐾! 𝑃!! 𝐾! 𝑃!"
𝐾! 𝑃!! 𝐾! 𝑃!"
In C programming language the equations can be written:
float P00_temp = P[0][0];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
On start-up it is necessary to have some variances pre-set as to allow the Kalman
filter to work within some constraints. These variables can be set manually by doing
various tests and calibrations; however, Kristian Lauszus has already performed
these calibrations and published the working values for other users. The following
variances have been found to work with most IMU’s, including the MPU-6050, which
he has used in some of his work [58]. The values are found to be:
Q_angle = 0.001f;
Q_bias = 0.003f;
R_measure = 0.03f;
All the equations from steps one to seven are written in C language to make a
source file for the Arduino IDE (Integrated Development Environment).

60
6.4 Arduino Libraries
The IDE allows users to type their own codes to produces programs for Arduino
boards. This short section will briefly discuss how a library can be made to support
an Arduino code.
A library is a set a instructions that Arduino IDE can understand, and thus make life
easier for beginners when wanting to write a program which maybe out of their
capability. For this project the Kalman filter library was downloaded from GitHub [58].
A library can be used in any code by specifying the library to be used at the
beginning of the code, by using:
#include “library_name.h”
An Arduino library consists of a source file, a header file, and a keywords file,
though the keywords file is not essential [59]. The source file contains the actual
code to make the program run; whilst the header file is basically a list of what is
inside [59]. The keywords document is a list of words that the user wishes to be
highlighted within the code; this does not affect the code in anyway, but it makes the
code easier to read and easier for the user to spot mistakes when writing the code
[59].
6.5 Kalman Filter Library
In this section of the report the Kalman filter library will be explained, including the
header file, source file and keywords file for the Kalman filter. This code has been
written by Kristian Lauszus for public modification and use [58]. The code will first be
presented and then the most important and relevant parts of the code will be
discussed.

61
6.51 Kalman Filter Header File (.h)
As previously explained, the header file is a list of everything inside the library, and
has the file extension “.h”; contains definitions for the library. The full header file is
written below. All codes are attached as appendixes at the end of the report.
#ifndef _Kalman_h_
#define _Kalman_h_
class Kalman {
public:
Kalman();
float getAngle(float newAngle, float newRate, float dt);
void setAngle(float angle); // Used to set angle, this should be set as the
starting angle
float getRate();
void setQangle(float Q_angle);
void setQbias(float Q_bias);
void setRmeasure(float R_measure);
float getQangle();
float getQbias();
float getRmeasure();
private:
float Q_angle; // Process noise variance for the accelerometer
float Q_bias; // Process noise variance for the gyro bias
float R_measure; // Measurement noise variance - this is actually the variance
of the measurement noise

62
float angle; // The angle calculated by the Kalman filter - part of the 2x1 state
vector
float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1
state vector
float rate; // Unbiased rate calculated from the rate and the calculated bias -
you have to call getAngle to update the rate
float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
};
#endif
From first inspection we can see that the whole code is sandwiched between three
lines of codes beginning with a hash “#”.
#ifndef _Kalman_h_
#define _Kalman_h_
…code goes here…
#endif
This “sandwich” is common practice for any header file in a library, and basically
stops any duplications or problems if someone accidentally includes the library twice
[59].
Next we see the lines:
class Kalman {
public:
The class of a code can be public, meaning that they are available to anyone using
the library, or they can be private, meaning that they can only be accessed from
within the class itself. Simply put, a class a collection of the functions and variables
used in library [59].

63
The rest of the code in the header file is simply listing all the variables that will be
used in the source file, such as: angle, bias and rate. The code also contains
different variables assignments including “float” and “int”. These assignments allow
the variables being defined to have certain characteristics, using more or less bytes,
and therefore memory. Variables assignments of type “int”, means the data is stored
as an integer in the range of -32,768 to 32,767; to store larger integers the
assignment “long” is used. If assigned “float” the variable data can be stored as a
floating-point number and if the variable is assigned “char” then the data can be
stored as a character [15]. Any writing after two forward slashes “//” is not read by
the program and instead is used to comment on a piece of code. To comment a
whole paragraph in code, we use “*/ and /*” to start and end the commented section
respectively [15].
6.52 Kalman Filter Source File (.ccp)
The source file is the actual code that relates to the header file and makes use of all
the variables in the header file. The code, again, has been written by Kristian
Lauszus and made available via GitHub. The code is shown over the next three
pages.
#include "Kalman.h"
Kalman::Kalman() {
/* We will set the variables like so, these can also be tuned by the user */
Q_angle = 0.001f;
Q_bias = 0.003f;
R_measure = 0.03f;
angle = 0.0f; // Reset the angle
bias = 0.0f; // Reset bias

64
P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting
angle (use setAngle), the error covariance matrix is set like so - see:
http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.0f;
};
// The angle should be in degrees and the rate should be in degrees per
second and the delta time in seconds
float Kalman::getAngle(float newAngle, float newRate, float dt) {
// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
// Modified by Kristian Lauszus
// See my blog post for more information:
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-
how-to-implement-it
// Discrete Kalman filter time update equations - Time Update ("Predict")
// Update xhat - Project the state ahead
/* Step 1 */
rate = newRate - bias;
angle += dt * rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
// Discrete Kalman filter measurement update equations - Measurement
Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */

65
float S = P[0][0] + R_measure; // Estimate error
/* Step 5 */
float K[2]; // Kalman gain - This is a 2x1 vector
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
// Calculate angle and bias - Update estimate with measurement zk
(newAngle)
/* Step 3 */
float y = newAngle - angle; // Angle difference
/* Step 6 */
angle += K[0] * y;
bias += K[1] * y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
return angle;
};
void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle,
this should be set as the starting angle
float Kalman::getRate() { return this->rate; }; // Return the unbiased rate
/* These are used to tune the Kalman filter */
void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };

Third Year Project Report

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to Third Year Project Report

Similar to Third Year Project Report (20)

Third Year Project Report