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.