Informatica Universiteit van Amsterdam. Combining wireless sensor networks with inertial navigation for improved spatial location.

Transcription

1 Bachelor Informatica Informatica Universiteit van Amsterdam Combining wireless sensor networks with inertial navigation for improved spatial location Jan Laan August 6, 212 Supervisor: Anthony van Inge, Universiteit van Amsterdam Signed:

2 2

3 Abstract This paper describes how a cluster of wireless nodes works together, sharing accelerometer data to improve the spatial accuracy of the entire cluster. Accelerometer data will be used to calculate position, and this will be combined with a Kalman filter to decrease the error of the system. The wireless signal strength of the nodes will be used as a second source of position data to further increase the accuracy. This combination results in a decrease in the error of the location. 3

4 Contents 1 Introduction Problem Description Sensor accuracy Platform Theory Position from accelerometers Problems with accelerometer data Position from wireless signal strength Kalman filters Accuracy measurements Accelerometer data Calibration No movement Constant acceleration Signal strength data Signal strength to distance Combining data with Kalman filtering Visualization Method Results Position over time Single node movement Conclusion 23 6 Further work 24 A Accelerometer data 25 A.1 Non-moving accelerometer A.2 Spinning wheel acceleration

5 CHAPTER 1 Introduction GPS based navigation is widely used in various applications and on many devices to determine the current position of a moving device. However, a major drawback of the GPS system is that a GPS receiver needs a direct line of sight to GPS satellites, therefore GPS is very inaccurate indoors. This document describes an approach to solve that problem and provide more accurate positional data indoors for moving entities, referred to as nodes throughout this text. 1.1 Problem Description There are multiple methods to determine position with one or more types of sensors. Each method has its own uses and drawbacks. For example, for a set location it is possible to create wireless nodes at known locations transmittting a signal, so they act as beacons. Mobile receivers can then pick up these signals. The position is then determined based on the signals picked up from these beacons. [1] This can be done with standard wireless equipment. This method works very well. The drawback of such a method is that the infrastructure needs to be in place, not every building has such an infrastructure. Sensors which can be used for determining position by inertial navigation, such as accelerometers, gyroscopes, all share the same drawback: They operate on the principle of dead reckoning. Any navigation attempt must start at a known location, after which a new position is determined based on the old location. This generally causes inaccurate navigation attempts.[2] Sensor accuracy There is not a single type of sensor available to accurately determine the position of a node. [3] Every available sensor has its drawbacks. To improve the accuracy multiple data sources can be combined. It will be attempted to do this with a cluster of nodes equipped with accelerometers, communicating wirelessly. This cluster can share data about the position of each node so that the cluster as a whole will have a more accurately known position. The assumption is that any node will have a random error, but in a combined cluster of nodes, these errors will on average even themselves out, resulting in a more accurate system. Additionally wireless signal strength can be used to measure 5

6 the distance between two nodes, combining this with the rest of the data will increase the accuracy even more. The question answerered in the rest of this text is: Can a cluster of nodes working together be more accurate in determining position than a single node? Subquestions are: What is the accuracy of the accelerometer and wireless signal strength? How can data from different nodes be combined to improve spatial accuracy? Does the improved accuracy stay improved over time? 1.2 Platform The platform used is a number of identical, individual sensor devices with wireless networking capability and a 3-D accelerometer. With these devices a network can be created where these devices can share their accelerometer data. This accelerometer data is used to calculate location. This project will be built with the Sun SPOT platform.[4] A SPOT is a small device with a range of sensors and functionalities which can all be accessed by Java libraries. In this project, the sensors used are the 3-D accelerometer, and the wireless signal strength which is measured each time a meassage is received wirelessly from another node. When two wireless devices communicate, a receiver can measure the power of the received signal. This measurement is called the received signal strength indication (RSSI), and is defined in the IEEE standard. [5] There are two types of SPOT nodes. The regular node is a wireless device with a battery which can be used for taking measurements from its sensors. The other type is the basestation, which is very similar, but does not have a battery or a sensor board. The basestation is usually attached to a computer with an USB cable. This way, the basestation can communicate with both the other SPOT nodes and a program on the computer. The basestation can read the data received from the other nodes, and a program on the computer can interpret this data and process it as required. 6

7 Figure 1.1: Platform layout 7

8 CHAPTER 2 Theory This chapter describes the theoretical background of this project. Both data sources used, the accelerometer and the wireless signal strength, will be described, as well as the method used to combine the data to create a more accurate set of data. 2.1 Position from accelerometers An accelerometer is a sensory device which measures acceleration, the rate of change of speed. The data given by accelerometers can be used to calculate the position of a single node, relative to its starting position. The method is as follows: An accelerometer gives the current acceleration in a certain direction, a series of these values can be integrated to find the speed of the node. i v i = v i 1 + a i di (2.1) i 1 v i is the velocity of a node at a time i in m/s, a i is its current acceleration in m/s 2. The interval from i 1 to i is the time between every measurement. A series of speed measurements can then be integrated again to find the relative location of the node. i d i = d i 1 + v i di (2.2) i 1 d i is the relative distance in m at time i. Using the above method for three accelerometers along three different axes, the relative position can be determined in three dimensions. This is a simple method, and in theory this works very well Problems with accelerometer data This theory holds in an ideal situation. However, in practice this method will usually result in very inaccurate positional data. This is due to several factors. [6] From the formulas given, it can be seen that each speed calculation relies on the result of the previous speed calculation. This kind of navigation is called dead reckoning. If any calculation result has a small error, due to inaccuracy of the sensor itself or an external factor, that error will accumulate into future calculations. This can cause errors to increase over time. The same reasoning applies to the calculation of position. 8

9 It is dependant on previous calculations of the nodes speed. An error in the speed will accumulate into further calculations. Because of this accumulation of errors, a small error in the calculation of speed will increase linearly. this linearly increasing error will accumulate into the position calculation, causing a quadratic error. This effect will be demonstrated in section There are two kinds of errors which contribute to the inaccuracy of the accelerometer. One is a systematic error, which causes the accelerometer to give a result with a constant error. This causes the acceleration to be too high or too low on average, which contributes greatly to the inaccuracy of the integrations of the position calculation, as described above. However, as this error is constant, it can be corrected by subtracting the error, once known, from each measurement. The other type of error is a random error. The accelerometer measurement is with the systematic error corrected not always correct. Due to external factors, such as temperature or vibrations, the accelerometer measurement has a random error each measurement. This error is not evenly distributed, it does not average to the correct acceleration. Therefor it can not be accounted for. With a cluster of nodes, there is more data available, and the assumption is that with this data combineed, the random error does combine into a more correct acceleration. In the next chapter it will be demonstrated that using just accelerometers works fine in theory, but in practice it is unusable for more than a few seconds of position estimation. More data will be needed to provide a reliable positioning system. 2.2 Position from wireless signal strength Wireless signal strength, RSSI, decreases over distance. Therefor RSSI can be used as an indication of distance between two nodes. The signal is stronger, the closer the two nodes are together. [7] In an ideal situation, an exact distance measurement can be calculated from this measurement. However there are many pitfalls which make the RSSI measurement unsuitable for location determination on its own. RSSI only measures distance in one dimension, only the distance between two nodes can be measured, not the position in relation to eachother. To do this, triangulation is needed. When there are at least three other known nodes in a known location, a node can determine its own location based on their position and the distance between the node and these known neighbour nodes. [8] It is assumed that the strength of the signal emitted by a node is equally strong in all directions. Measurements have shown this is not the case for the Sun SPOT devices. [9] This results in different values being measured while the distance between nodes is the same. Another problem is wireless interference. Other wireless signals, for instance another wireless network, or even a microwave, can interfere and disrupt the signal. [1] Any obstacle such as walls, people, etc. have all sorts of effects on wireless signals, from completely blocking the signal to decreasing, or even increasing the signal strength under some conditions. [9] Lastly, while the method of how and when to read RSSI is defined in the standard, what the values actually mean is not defined. Different vendors use different scales for RSSI, so these values are not always interchangeable. Because a single platform is used, this is not a problem. However, this does mean that their is no universally defined relation between signal strength and distance. This is a problem that can be overcome by measuring that relation for a specific device. 9

10 With all these uncertainties, only an approximate distance measurement from the signal strength can be obtained, which will also only work for one certain device type. Clearly, this is not suitable for providing accurate location data. The advantage this method has over reading the accelerometers is that the error does not accumulate over time. While the error is unknown, and can vary over time, measurements do not depend on previous measurements, so the error will not increase linearly of quadratically over time. 2.3 Kalman filters As shown in the previous two sections, both accelerometers and wireless signal strength are not suitable for accurate location determination on their own. By combining these two data sources a combined system will be created with improved accuracy. Nodes can combine their accelerometer data to create a cluster of nodes which is more accurate than a single node, afterwards the signal strength data can be used to further increase the accuracy of the calculation. A well known and widely used method for combining multiple data sources is a Kalman filter. [11] A Kalman filter is a recursive algorithm which attempts to eliminate noise from a series of measurements, and to create a more precise estimate of the measured value. There are multiple variants of a Kalman filter, but the variant used in this work is a simple discrete Kalman filter. It can be described as using a two-step process, using the steps predict and update. At some time, the state (position and speed in our case) will be calculated. Then, in the prediction step, the filter will advance the model in time, and attempt to predict the next next state. The second step is the update step. Here the prediction will be updated with an (inaccurate) measurement, to improve its accuracy. Given a known location x k 1, predict the position at x k using the accelerometer, along with its error P k. The indicates this is only a prediction, an intermediate value. x k = A k 1 x k 1 + B k u k P k = A k 1 P k 1 A T k 1 + Q k 1 where A is the prediction, the acceleration. u is a control input, irrelevant in our version. Q is the noise covariance. The error of the prediction is assumed to be normally distributed with p(w) N(, Q) Then the Kalman gain is determined, which is the ratio between the error of the prediction (accelerometer) and the error of the measurement (signal strength): K k = P kh T k (H k P kh T k + R k ) 1 Where R is the error covariance of the measurement (the wireless signal), similar to Q. Update prediction and error based on wireless signal strength: x k = x k + K(z k H k x k) P k = (I K k H k )P k z is the actual measurement, I the identity. After this, time is advanced one step, and the process is repeated. 1

11 CHAPTER 3 Accuracy measurements 3.1 Accelerometer data As described previously, the accelerometer provides inaccurate data. extent of the inaccuracy, this was measured. To confirm the Calibration Some accuracy can be gained by calibrating the accelerometers before starting any measurement. This is done to minimize the constant offset error of the device. At the start of each session, when the node is lying still, the acceleration in each direction is measured 1 times in succession. The average of this is considered the offset of the accelerometer, which is then removed, so each accelerometer has an acceleration of when the device starts tracking. The effects of this calibration can be seen in figure 3.1 Without calibration the is.14m/s 2, with calibration it is.3m/s 2. without calibration with calibration (a) Accelerometer without calibration (b) Accelerometer with calibration Figure 3.1: Accelerometer data over time No movement First, the data retrieved from the accelerometer of the node without movement was measured. Ideally, this should be m/s 2. A typical result can be seen in figure 3.2a. As can be seen, the measured acceleration is continuously changing, but always around 11

12 the m/s 2 point. It also shows that the resolution of the sensor is.5m/s 2. The acceleration is distributed as seen in figure 3.2b. The is.3m/s 2 in this case, with the maximum measured acceleration.1m/s 2 above the average, and the minimum acceleration.16m/s 2 below the average. These values vary from device to device. When these values are integrated to find speed, results are those as shown in figure 3.2c. While the acceleration remains constant with a small error, the speed increases linearly. This is a result of the of.3m/s 2 Using equation 2.1, the speed after 12 seconds is 3.6m/s. Results of integrating the speed to find the position are in figure 3.2d. Using equation 2.2, the distance after 12 seconds is 216m. This increase in position is quadratic. This error was predicted in the theory section. This set of results show typical behaviour for these devices. other axes and other devices are shown in appendix A.1. Similar results from X axis distribution of accelerometer number of occurences time (a) Typical accelerometer reading X axis speed over time accelerometer value (b) Distribution of accelerometer values X axis speed over time position over time Speed (m/s) Position (m) time (c) Typical accelerometer speed increase (d) Typical accelerometer position increase. Figure 3.2: Development of integration of accelerometer data over time Constant acceleration To determine if the acceleration error from the node remained constant at higher accelerations, a node was strapped to a bycicle wheel which was spun, to create a constant acceleration due to the centrifugal force. Results are in figure 3.3a 12

13 number of occurences distribution of accelerometer time (a) Constant high accelerometer value (b) Distribution of acceleration The error, at this much greater acceleration has actually improved. It is now at most.4m/s 2 below the and. The distribution of the acceleration is also more constant, as can be seen in figure 3.3b. The acceleration alternates between only two values, unlike the acceleration graph without motion (figure 3.2a). Multiple tests showed similar results, see appendix A.2 The conclusion from this is that the accelerometer is more accurate by a factor 4 at high speeds. 3.2 Signal strength data In order to see if signal strength data was indeed a reliable (albeit inaccurate) source of distance data, a series of measurements were performed to see if the measured RSSI values is constant over a period of time. The measurements are shown in figure signal strength (cm) signal strength (1cm) signal strength (1cm) signal strength (2cm) Signal Strength (RSSI) Figure 3.3: Wireless signal strength over time for different distances 13

14 Histogram of signal strength Occurences Signal strength (RSSI) Figure 3.4: Histogram of wireless signal strength for different distances. From left to right: 2cm, 1cm, 1cm and cm As can be seen, duee to external factors mentioned in section 2.2, the signal strength fluctuates even when the distance between two nodes is constant. In the test shown above, the RSSI was up to 4 above or below the average RSSI value for any distance, with the exception of one occurence at 1cm where RSSI was 7 below average. This shows the relation between distance and signal strength, but it also shows its inaccuracy. The average signal strength at 1cm and 1 cm only differs 2 in RSSI. With an error of 4 in RSSI, these distances can be confused with eachother, which can also be seen in the graph above, where the 1cm and 1cm measurements overlap Signal strength to distance As described in the theory section, the relation between RSSI and distance needs to be mapped for each wireless device. Tthis was done by creating a simple program on a node, which broadcasts some random data every second. On the basestation side the RSSI value was obtained from the node and the values were saved. The two devices were then placed at a range of set distances from eachother, randing from cm to 2 cm. Ten measurements were performed for each distance. This resulted in a scatterplot of a couple hundred measurements. These values were fitted using a least squares polynomial fit to obtain a 4th defree function. 14

15 2 1 fitted function measured values Signal strength (rssi) Distance (cm) The resulting 4th degree function is as follows. D = x x 3 +.1x x This function is used throughout the rest of the project wherever a distance needs to be obtained from a RSSI value. 3.3 Combining data with Kalman filtering There are now two sources of data available, accelerometers and wireless signal strength. Both of these sources have a certain error, as shown in the previous sections. To be able to know our location more accurately, and decrease the overall error, this data will be combined from both sources and multiple nodes. The method used will be a Kalman filter, as described in section 2.3. The position calculated from the data given by the accelerometer of a node will be used as the prediction step of our model, the distance retrieved from the RSSI of the wireless signal of neighboring nodes will serve as correction step to improve the prediction. This goes as follows: At time i, the position and speed of the node are known. (At time they are both.) Integrating over the past interval yields the speed and position for the prediction step of the Kalman filter. Data from other nodes is constantly being received. Broadcast is the assumed location of a node. For each neighbouring node the distance between the node and its neighbour is calculated by comparing the predicted position of the node and the position broadcast by the neighbour. Additionally, the distance is calculated from the signal strength. Now there are two distance sources. As the Kalman filter dictates, a weighted 15

16 average is taken from these two, to determine the more accurate distance. The position of the node is updated accordingly, and is now more accurate. After that, the next neighbour node is read, and the process is repeated. The updated position from before is now a new prediction, the difference is that it is more accurate to start with. Each neighbour is processed in this fashion, continuously increasing the accuracy. Therefore more connected nodes in the network make for a higher accuracy. 16

17 CHAPTER 4 Visualization 4.1 Method To visualize the accuracy of our system, a simple 3D application was created, demonstrated in figure 4.1, where the position of all nodes in use can be shown. This was instrumental in getting an immediate idea of how the system performed. This application consists of a single 3D grid, representing the x,y and z axes, 5m in each dimension. The distance between grid lines is 1m, and the origin is in the center of the grid. In the grid, red dots represent the current position of any known nodes. This allowed us to quickly see the behavior of our nodes, and thus estimate the effectiveness of our algorithm. 17

18 Figure 4.1: Application; Red dots indicate active nodes, white dots are illustrative to show the grid layout. Initial testing was done without communication with other nodes, just one single node, using its accelerometer values to estimate its position. As can be seen in figure 4.2, this method of location tracking immediately showed its uselessness, while not moved at all, our node appeared to move. In the figure the position of the node was recorded every second, illustrated by the red dots. This simulation lasted for 22 seconds. Starting at, at the top right, the mode moved to the bottom left of the image, showing a negative movement in all axes. The quadratic increase in speed can be seen by the nodes lying farther apart when farther away from the starting position. The predicted lack of accuracy and increasing error of the accelerometer method is clearly shown by this. After 22 seconds, the x axis showed an error of 37m, the y axis of 73m and the z axis an error of 51m. Repeating of the same test, inclduing with different devices showed similar results. 18

19 1 2 3 Z axis X axis Y axis Figure 4.2: Calculated position (in cm) after initial test around each axis, shown for 22 seconds. After implementing the kalman filter, several tests were ran with a number of nodes activated. 1. Four nodes lying still, next to each other. 2. Again four nodes, but with most of the nodes lying still, and one node being moved back and forth along one axis. 4.2 Results Position over time In this test the effect on the accuracy of the combined data of the cluster of nodes was tested. Here four nodes were used together. All of the nodes were placed next to eachother, without being moved during the test. As demonstrated before, in section 4.1, a single node without the Kalman fiter had moved over m after 2 seconds while lying still. Image 4.3 shows a group of nodes who share their data after 2 seconds. The part of the grid in the image is the origin of the grid. All the nodes have stayed together in a group, while moving upward. The uppermost node is.1m away from the origin in the x direction,.5m in the y direction, and.2m in the z direction. The other three nodes have similar values. The accuracy of the original approach, using just the accelerometer, and the approach with the cluster are compared in table

20 Time (s) distance (original) (cm) distance (improved) (cm) Table 4.1: Comparison of distance over time for a single node and the cluster of nodes, with the actual distance remaining at. Figure 4.3: Four nodes after 3 s without movement. All nodes start at the center of the grid, the image shows the nodes after 3 seconds. As can be seen, they have not moved far from the center. In fact, no node is farther than 1m from the center. What also can be seen is that the nodes stay together, as a single group. The combination of the errors of the nodes causes the average error to be smaller, as shown above. Also, in this situation with none of the nodes moving, the signal strength remains constant (with its own error), indicating no change in position, thus further preventing the nodes from drifting apart. However, because there is no reference point other than the nodes, they slowly drift away from the center as a group Single node movement In this test it was measured how the nodes behaved during movement, to see if the increased accuracy was achieved. This measurement consists of moving one node along a single arbitrary axis. The node was moved back and forth in a single direction. To create a consistent test, a single node was placed on a cart on a section of track. A rotation sensor was also attached to the moving cart. With this sensor the distance 2

21 travelled by the cart can be accurately logged. This is used as a reference for the distance calculation performedd by the node itself. This calculation can be compared with the reference to determine the accuracy. This setup yielded the following results: Figure 4.4: Test setup accel. data distance 2 1 combined data signal str. data accel. data distance (a) Result with only accelerometer used (b) Result with Kalman filter approach Figure 4.5: Improvement for movement along a single axis. The red line in the figures is the reference movement. With only the accelerometer used, no other nodes, results were as in figure 4.5a. The reference moves 1m in a single direction and back again, but the acccelerometer calculation from the node moves 35 m. The combination of the accelerometer with the signal strength measurement improves this, as can be seen in figure 4.5b which shows the position of one node in the cluster. The green line represents the distance calculated from the signal strength alone, the yellow is the accelerometer of this node. The blue line is the combination of the data from the cluster with the Kalman filter. The red line is the reference distance. The calculated position has a small error for the first half of the movement. At the farthest distance from the start, the error is 12 cm, which is also the greatest error up 21

22 to that point. In the second half of the movement, when moving the cart back, the error increases gradually. At the end of the movement, the error has increased to 7 cm. 22

23 CHAPTER 5 Conclusion Accurate indoor navigation still has no universal solution like GPS. The presented solution attempts to resolve this. The accuracy of an accelerometer in itself is insufficient after more than a few seconds due to the accumulating error. Wireless signal strength fluctuates too much to be accurate enough. Data can be effectively combined by using Kalman filter. The accuracy of the system with a cluster of nodes working together using this filter is better than a single node by itself; instead of an error of m or more, the error after 2 seconds is reduced to 1m. (table 4.1) Systematic errors are eliminated, the remaining error is random. This causes the cluster to drift in a random direction. Using the signal strength as an additional data source helps in keeping the nodes of the cluster together at the correct distance from eachother, but does nothing to reduce the random drift. As a whole, the accuracy of the system does increase by combining all the data. 23

24 CHAPTER 6 Further work Improvements can be made upon the presented system in various ways. One of the major problems from a practical point of view is the inability for the system to determine its rotation. Without this, there is no way of knowing in which direction the system is headed, so a turn cannot be detected. This could be resolved by adding a 3D compass or gyroscope to the system. This will allow rotation to be tracked accurately. This information can be used to determine turns and take these into account, soo the system knows not only how far it is going, but also in which direction. Also a possibility is to use wireless triangulation to determine the position of the nodes in 2D or 3D with the wireless signal alone. This might improve accuracy even more, but is hard to do as their are no reference points for the triangulation, only the nodes in the system. Measurements have only been performed in environments without obstructions as walls and humans. How the system performs with these obstructions is at this moment unknown. 24

25 APPENDIX A Accelerometer data A.1 Non-moving accelerometer X axis Y axis Z axis (a) X-axis accelerometer data X axis (b) Y-axis accelerometer data Y axis (c) Z-axis accelerometer data Z axis speed over time speed over time position (m) 1 speed (m/s) 1 speed (m/s) speed over time (d) X-axis speed data X axis (e) Y-axis speed data Y axis (f) Z-axis speed data Z axis speed over time position over time speed over time position over time position (m) position (m) 1 1 speed over time position over time (g) X-axis position data (h) Y-axis position data (i) Z-axis position data Figure A.1: Measured acceleration, speed and position without movement for device 3D77 25

26 X axis Y axis Z axis (a) X-axis accelerometer data X axis (b) Y-axis accelerometer data Y axis (c) Z-axis accelerometer data Z axis speed over time speed over time speed over time position (m) 1 speed (m/s) 1 speed (m/s) (d) X-axis speed data X axis 4 (e) Y-axis speed data Y axis 4 (f) Z-axis speed data Z axis speed over time position over time position (m) position (m) 1 1 speed over time position over time speed over time position over time 2 (g) X-axis position data (h) Y-axis position data (i) Z-axis position data Figure A.2: Measured acceleration, speed and position without movement for device 6619 A.2 Spinning wheel acceleration This test involved strapping a Sun SPOT to a bycicle wheel and spinning that to create a high centrifugal force. This force was measured and several measurements are shown in the images below. 26

27 time time time Figure A.3: Constant high acceleration obtained from centrifugal force. 27

28 Bibliography [1] Sybren Stuvel. Sadako - securing a building using ieee Universiteit van Amsterdam, 26. [2] Cliff Randell, Chris Djiallis, and Henk Muller. Personal position measurement using dead reckoning. Proceedings of the Seventh International Symposium on Wearable Computers, IEEE Computer Society, pages , 23. [3] Xiaoping Yun, Eric R. Bachmann, Hyatt Moore IV, and James Calusdian. Selfcontained position tracking of human movement using small inertial/magnetic sensor modules. IEEE Conference on Robotics and Automation, pages , April 27. [4] Sun spot world. [5] IEEE Standard for Low-Rate Wireless Personal Area Networks. Ieee standard , 23. [6] Roger Meike. Location, location, location (accelerometer). location location accelerometer, november 28. [7] Kamin Whitehouse, Chris Karlof, and David Culler. A practical evaluation of radio signal strength for ranging-based localization. SIGMOBILE Mob. Comput. Commun. Rev., 11(1):41 52, January 27. [8] Yahong Way, Boon-Poh Ng, and See C.-M.S. A real-time indoor wifi localization system utilizing smart antennas. IEEE Transactions on Consumer Electronics, 53: , 27. [9] Roger Meike. Location, location, location (radio). location location radio, august 29. [1] A.H. Sayed, A. Tarighat, and N. Khajehnouri. Network-based wireless location: challenges faced in developing techniques for accurate wireless location information. Signal Processing Magazine, IEEE, 22(4):24 4, july 25. [11] Greg Welsh and Gary Bishop. An introduction to the kalman filter