extended-kalman-filter

Apply Extended Kalman Filter to track the motion of object by #MATLAB#

This project is to implement Extended Kalman Filter to estimate motion states of the object based on the radar measurements, also including: linearize the measurement system, analyze estimations obtained.

Due to the copyright of the assignment, I cannot show the questions and my complete answers here.

Just give a short description:

the Kalman Filter tries to estimate the state π‘₯ of a discrete-time controlled process which is constrained by linear stochastic difference equations. But what happens if the process to be estimated and/or the measurement relationship are/is non-linear? Like below equations:

                                        π‘₯(π‘˜+1) = 𝑓(π‘₯(π‘˜),𝑒(π‘˜),𝑀(π‘˜))         
                                        𝑧(π‘˜)=β„Ž(π‘₯(π‘˜),𝑣(π‘˜))                   
                         where the random variables 𝑀(π‘˜)and 𝑣(π‘˜) again represent the process and measurement noise.

The solution is to linearize the function about the current mean and covariance, and a filter that works is called an Extended Kalman Filter(EKF). We can linearize the estimation by using the partial derivatives of the process and/or measurements to compute estimates.

In summary, in this project, the system we should solve out is with linear process equations and nonlinear measurement equations, respectively are:

                                        π‘₯(π‘˜+1)=𝐴π‘₯(π‘˜)+𝑀(π‘˜)           𝑀(π‘˜)~𝑁([0  0  0  0]𝑇, 𝑄)      (1)
                                        𝑧(π‘˜)=[β„Ž1(π‘₯(π‘˜),𝑣(π‘˜))     β„Ž2(π‘₯(π‘˜),𝑣(π‘˜))]𝑇 = β„Ž(π‘₯(π‘˜),𝑣(π‘˜))      (2)

The time updation equations are:

                                         π‘₯Μ‚βˆ’(π‘˜) = 𝐴π‘₯Μ‚βˆ’(π‘˜βˆ’1) + 𝐡𝑒(π‘˜βˆ’1)                (3)
                                         π‘ƒβˆ’(π‘˜) = π΄π‘˜π‘ƒ(π‘˜βˆ’1)π΄π‘˜π‘‡ + 𝑄𝑓(π‘˜βˆ’1)             (4)
                         where π΄π‘˜ is the process Jacobians at step π‘˜ and 𝑄𝑓(π‘˜) is the process noise covariance at step π‘˜

The measurement updation equations are:

                                         𝐾(π‘˜)=π‘ƒβˆ’(π‘˜)πΆπ‘˜π‘‡[πΆπ‘˜π‘ƒβˆ’(π‘˜)πΆπ‘˜π‘‡+𝑅𝑓(π‘˜)]βˆ’1       (5)
                                         π‘₯Μ‚(π‘˜)=π‘₯Μ‚βˆ’(π‘˜)+𝐾(π‘˜)[𝑧(π‘˜)βˆ’β„Ž(π‘₯Μ‚βˆ’(π‘˜),0)]          (6)
                                         𝑃(π‘˜)=[πΌβˆ’πΎ(π‘˜)πΆπ‘˜]π‘ƒβˆ’(π‘˜)                      (7)
                        𝐼 is the identity matrix, β„Ž isthe function in (2), 𝑅𝑓(π‘˜) is the measurement noise covariance at step π‘˜. The Jacobian πΆπ‘˜ at time step π‘˜ is given by:
                                       
                                       πΆπ‘˜[𝑖,𝑗] = πœ•β„Ž[𝑖]πœ•π‘₯[𝑗](π‘₯Μ‚βˆ’(π‘˜),𝑒(π‘˜βˆ’1))
                        Thus, 𝐢 is the Jacobian matrix of partial derivatives of β„Ž with respect to the state π‘₯.

Implement and apply the Extended Kalman Filter to the two given measurements in 'RadarMeasurements.mat'. Set 𝜎q2=1. Use 𝜎r2=1 to analyse the first measurement and 𝜎r2=100 to analyse the second measurement. See the code extended_km.mlx

Here I also attach the figure of motion track of the object, and the figure of Kalman gains obtained, and the figure of NIS plotting.