CN103889047A - Target tracking algorithm based on Kalman filtering - Google Patents

Abstract

The invention pertains to the target tracking field in a wireless sensor network, and discloses a method which can be used to perform real-time tracking on a target. According to the invention, at first, Kalman filtering is performed on a linear system, and then the linear system is extended to a nolinear system, and the problem is converted into an approximate linear filtering problem through linearization to obtain optimized extended Kalman filtering and non-trace Kalman filtering.

Description

A kind of target tracking algorism based on Kalman filtering

Technical field

The invention belongs to the target tracking domain in wireless sense network, can follow the tracks of moving target fast and accurately.The present invention is not only applicable to linear system, very effective to non linear system yet.Apply the present invention in wireless sensor network, can follow the tracks of processing to the node of any motion, will have very vast application prospect.

Background technology

Usually, the system equation of target following can represent with following formula:

X(k+1)＝ΦX(k)+u(k)

Z(k)＝HX(k)+v(k)

In Kalman filtering, the one-step prediction equation of state estimation is:

$\hat{X}(k+1|k)=\mathrm{\Φ}\left(k\right)\hat{X}\left(k\right|k)+u\left(k\right)$

The error equation of one-step prediction is:

$\tilde{X}(k+1|k)\≅X(k+1)-\tilde{X}(k+1|k)=\mathrm{\Φ}\left(k\right)\tilde{X}\left(k\right|k)+G\left(k\right)W\left(k\right)$

Its one-step prediction covariance is:

$P(k+1|k)\≅E\left[\tilde{X}(k+1|k){\tilde{X}}^k(k+1|k)\right|Z^k]=\mathrm{\Φ}\left(k\right)P\left(k\right|k){\mathrm{\Φ}}^T\left(k\right)+G\left(k\right)Q\left(k\right)G^T\left(k\right)$

In formula,

for the state estimation error covariance of k moment transducer, Z ^k=Z (j), and j=1,2 ..., k} is k moment cumulative measurement vector.

In like manner, the observation vector of prediction is:

$\hat{Z}(k+1|k)=H(k+1)\hat{X}(k+1|k)$

The predicated error of observation vector or be called new breath and be:

$\mathrm{\ϵ}(k+1)=Z(k+1)-\hat{Z}(k+1|k)$

$=Z(k+1)-H(k+1)\hat{X}(k+1|k)$

So the predicting covariance of observation (also claiming new breath covariance matrix) is:

$S(k+1)=E\left[\tilde{Z}(k+1|k){\hat{Z}}^T(k+1|k)\right|Z^k]$

$=H(k+1)P(k+1|k)H^T(k+1)+R(k+1)$

According to new breath and new breath covariance matrix, can obtain the gain of Kalman filter:

K(k+1)＝P(k+1|k)H ^T(k+1)S ^-1(k+1)

＝P(k+1|k)H ^T(k+1)/[H(k+1)P(k+1|k)H ^T(k+1)+R(k+1)]

So the state renewal equation of Kalman filtering algorithm is:

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)\mathrm{\ϵ}(k+1)$

Correspondingly, the error covariance renewal equation of filter can be expressed as:

P(k+1|k+1)

＝P(k+1|k)-P(k+1|k)H ^T(k+1)S ^-1(k+1)H(k+1)P(k+1|k)

＝P(k+1|k)-K(k+1)S(k+1)K ^T(k+1)

＝[I-K(k+1)H(k+1)]P(k+1|k)

Summary of the invention

The object of the present invention is to provide a kind of method of in real time target being followed the tracks of.Specific implementation comprises the following steps:

(1) target that is linear system to the equation of motion is carried out standard Kalman filter tracking;

(2) target that is non linear system to the equation of motion is carried out EKF tracking;

(3) target that is non linear system to the equation of motion is carried out Unscented kalman filtering tracking.

The invention has the advantages that tracking effect is remarkable, speed is fast, and applied range.

Accompanying drawing explanation

Fig. 1 standard card Kalman Filtering pursuit path figure;

Fig. 2 standard card Kalman Filtering tracking error figure;

Fig. 3 EKF pursuit path figure;

Fig. 4 EKF tracking error figure;

Fig. 5 Unscented kalman filtering pursuit path figure;

Fig. 6 Unscented kalman filtering tracking error figure.

Embodiment

Below in conjunction with accompanying drawing and instantiation, the present invention will be further described:

A) EKF:

Kalman filtering can, under the condition of linear Gauss model, can be made optimum estimation to the state of target, obtains good tracking effect.To utilize linearisation skill to be translated into an approximate linear filtering problem to the conventional processing method of Nonlinear Filtering Problem.Therefore, can utilize the local linear characteristic of nonlinear function, by nonlinear model Linear localization, recycling Kalman filtering algorithm completes filter tracking.Expansion Kalman filtering is exactly the thought based on such, the nonlinear function of system is done to single order Taylor and launch, and completes the processing such as filtering estimation to target thereby obtain linearizing systems approach.

Nonlinear Systems ' Discrete dynamical equation can be expressed as:

X(k+1)＝f[k，X(k)]+G(k)W(k)

Z(k)＝h[k，X(k)]+V(k)

Here for the ease of Mathematical treatment, suppose the input that there is no controlled quentity controlled variable, and suppose that process noise is that average is zero white Gaussian noise, and noise profile matrix G (k) is known. wherein, observation noise V (k) is also that additivity average is zero white Gaussian noise.Suppose that process noise and observation noise sequence are independent of each other.

B) Unscented kalman filtering:

Without mark Kalman filtering (Unscented Kalman Filter) [23], a kind of nonlinear filtering mode that the people such as S.Julier propose. different from expansion Kalman filtering is, it does not do linearisation to nonlinear equation F and h at estimation point place and approaches, near estimation point, determine sampling, the probability density function of the approximate state of the gaussian density representing with these sample points without mark conversion UT (Unscented Transform) but utilize. can be summarized as the following steps without mark Kalman filtering algorithm:

First, calculate the state one-step prediction that Sigma is ordered:

X ⁽ⁱ⁾(k+1|k)＝f[k，X ⁽ⁱ⁾(k|k)]

System mode one-step prediction and covariance matrix are:

$\hat{X}(k+1|k)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}^{\left(i\right)}{X}^{\left(i\right)}(k+1|k)$

$P(k+1|k)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}^{\left(i\right)}{[\hat{X}(k+1|k)-X^{\left(i\right)}(k+1|k)][\hat{X}(k+1|k)-X^{\left(i\right)}(k+1|k)]}^T$

The observation that Sigma is ordered is predicted as:

Z ⁽ⁱ⁾(k+1|k)＝h[X ⁽ⁱ⁾(k+1|k)]

Observation prediction average and covariance are:

$\stackrel{\‾}{Z}(k+1|k)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}^{\left(i\right)}{Z}^{\left(i\right)}(k+1|k)$

$S(k+1)=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}^{\left(i\right)}[\hat{Z}(k+1|k)-Z^{\left(i\right)}(k+1|k)]{[\hat{Z}(k+1|k)-Z^{\left(i\right)}(k+1|k)]}^T$

Gain matrix is: $K(k+1)=\left\{\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}^{\left(i\right)}\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}^{\left(i\right)}\right[\hat{X}(k+1|k)-X^{\left(i\right)}(k+1|k)\left]{[\hat{Z}(k+1|k)-Z^{\left(i\right)}(k+1|k)]}^T\right\}{S}^{-1}(k+1)$ After renewal

System state estimation and covariance matrix are:

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)[Z(k+1)-\hat{Z}(k+1|k)]$

P(k+1|k+1)＝P(k+1|k)-K(k+1)S(k+1)K ^T(k+1)?。

Claims (4)

1. a real-time method to target following, is characterized in that, the method contains following steps:

(1) carry out state estimation one-step prediction

(2) calculate one-step prediction covariance P (k+1|k);

(3), according to new breath and new breath covariance matrix, can obtain the gain K (k+1) of Kalman filter;

(4) result of basis (3) obtains the state renewal equation of system

(5) upgrade error covariance matrix P (k+1|k+1).

2. method for tracking target according to claim 1, is characterized in that: Kalman filtering is exactly optimal filter, and its system of processing Gauss model is also very effective.

3. in non linear system, conventional processing method is to utilize linearisation skill to be translated into an approximate linear filtering problem, the thought of EKF that Here it is.It is characterized in that: can utilize the local linear characteristic of nonlinear function, by nonlinear model Linear localization, recycling Kalman filtering algorithm completes filter tracking.

4. Unscented kalman filtering is different from EKF, and it does not do linearisation to nonlinear equation F and h at estimation point place and approaches, and determines sampling but utilize without mark conversion UT near estimation point.It is characterized in that: average and the covariance of the Sigma point set obtaining are mated with former statistical property, directly these Sigma point sets are being carried out to Nonlinear Mapping, approximate state probability density function, this being similar to its essence is a kind of statistical approximation but not solution.

Images