Kalman Filter - Made Easy - 2/3

?

Let’s consider the case of a moving object and let’s assume that we want to predict the next state in terms of X location and ??velocity in X direction namely.In that case the state matrix X can be represented as

Using Newton’s law of motion we can predict next state of the system (at time t) ?using following equation :

Let’s see how we can make use of ?this equation using various matrices in Kalman Filter . Kalman Filter equation for prediction of next state of the system is as under

Where A is state transition Matrix, U?is control input and W ?is predicted state noise matrix .

Let’s define A as follows , where ?is the time in seconds ?between two successive iterations of Kalman filter

We can now calculate

As you can notice above that by solving equation ?we have got a new estimation of the state as :

If we use only this part of state estimation , we will land up in two issues namely

·???????? No update in velocity estimation

·???????? Error in position estimation due to assumption of constant velocity which may not be true , because driver can press accelerator pedal or apply brake resulting into some acceleration a

In order to take care of these issues we need to ?use ?the control variable Uk ?, which in this case will be acceleration – a

Now lets define matrix B as follows

After calculating the term B?* Uk, we can now have full prediction of the state as below

Where you can notice that both position and velocity are updated ?.

For simplicity purpose I considered ?predicted state noise matrix as 0 . However it needs to be set to some non-zero value , as there will always be some error in measurement of acceleration ( control input) . We will skip the discussion of how to determine the value of ?as it is out of scope of this technical article

Now let’s throw some light on ?– The process covariance matrix and Q – the process noise co-variance matrix . matrix represents the error in estimation of next state. The matrix Q is noise in process .

Matrix Q shall never be set to 0 since it prevents ?(Matrix P error in estimation) to become 0 . The problem which we will face when matrix P? becomes 0 is , we will always be using predicted state of the system without making any use of measurements . Some of the? practical reasons why Q can never be 0 are as under :

·???????? Wind Velocity

·???????? Change in Coefficient of surface friction

·???????? Sudden change in road gradient

However to simplify the things , I will assume it to be 0 . The P matrix in our example will get defined as follows

For simplicity purpose lets assume that we discard co-variance terms in the matrix and treat the matrix as the one with only variance terms

Moving forward we can now calculate ?the Process covariance matrix ( the error in state estimation) by using following equation

?

We will take a look at how to calculate a Kalman Gain and how to find out the next state of the system in third and last article in this series .