CN109548141B - Indoor environment base station coordinate position calibration method based on Kalman filtering algorithm - Google Patents

Abstract

The invention discloses a coordinate position calibration method of an indoor environment base station based on a Kalman filtering algorithm, which comprises the steps of constructing an indoor positioning system by a wireless sensor, and solving the horizontal distance between a label and the base station by adopting a geometric relation; all base stations and tags are provided with synchronous clocks, the base stations send wireless signals containing sending time timestamps to the periphery, the tags receive the wireless signals, and the distances between the tags and the base stations are calculated through time differences; setting the number and coordinates of base stations arranged in a space, calibrating four coordinate points which are connected into a rectangle in the space, moving a moving target along the coordinate points, and acquiring measurement distance information between the moving target and all the base stations in each sampling period; measuring the distance to each base station at an initial point by using a moving target, and preliminarily estimating the position of each base station; and obtaining the coordinate position information of the optimized base station through a two-step extended Kalman filtering algorithm. The method can accurately finish the calibration of the base station position in the indoor complex environment and improve the positioning precision.

Description

Indoor environment base station coordinate position calibration method based on Kalman filtering algorithm

Technical Field

The invention relates to a method for calibrating the coordinate position of an indoor environment base station based on a Kalman filtering algorithm.

Background

The mobile robot represents a higher level of mechatronics, and the wide application of the robot is based on autonomous navigation as a premise, but the navigation precision cannot be separated from higher positioning precision. The positioning and navigation functions of the robot can be well completed by arranging the sensor network in an indoor environment.

In the prior art, a plurality of distance equations are formed by a plurality of distance information, and the coordinates of a target point are obtained by using a least square method, so that the residual error of the equation is minimum; however, due to the complexity of the indoor environment, the sensor network may generate a large difference between the obtained distance information and the actual distance information due to noise interference in the ranging process, so that the positioning information may have a large deviation. Therefore, in the case of measurement errors, ensuring the positioning accuracy becomes a problem to be solved by the present invention.

Most of the current positioning algorithms aim to know the position of a base station so as to estimate the position information of a measured target. In larger spaces, however, it becomes a significant challenge to calibrate the location of the base station, and the most classical approach is to place the moving object at three fixed and known locations and then estimate the location of the base station using three-point measurements. Due to the existence of noise, the method cannot obtain very accurate position information, and the positioning precision is poor.

Disclosure of Invention

The invention aims to solve the technical problem of providing an indoor environment base station coordinate position calibration method based on a Kalman filtering algorithm, which overcomes the defects of the traditional positioning method, can accurately complete base station position calibration in an indoor complex environment, has strong anti-interference capability and effectively improves the positioning precision.

In order to solve the technical problem, the method for calibrating the coordinate position of the indoor environment base station based on the Kalman filtering algorithm comprises the following steps:

step one, setting an indoor positioning system based on a wireless sensor network, wherein single target positioning is composed of a label and a plurality of base stations, the label and the base stations adopt ultra-wideband modules to realize ultra-wideband information transmission, all the base stations are installed on the same horizontal plane, a positioned target is positioned at a height with the horizontal plane, the label is installed on a mobile robot, and the horizontal distance between the label and the base stations is solved by adopting the geometric relation of a right triangle:

wherein d is the distance between the label and the base station, Δ h is the height difference between the label and the base station, d_{Level of}Is the horizontal distance of the tag from the base station;

step two, ranging between each base station and each label adopts a TOF principle, synchronous clocks are set for all the base stations and the labels, the base stations send wireless signals to the periphery, the wireless signals comprise timestamps of sending time, when the labels receive the wireless signals, the tags compare the timestamps of the labels with the timestamps of the received wireless information, and the distance between the labels and the base stations is calculated through time difference:

d＝c·(t_receive-t_send) (2)

where d is the distance between the tag and the base station, c is the propagation speed of light in vacuum, t_receiveTime of reception of radio signal, t_sendTime of transmitting the wireless signal;

step three, noise exists in the distance information between the label and the base station, and the description is as follows:

dis(t)＝d_real(t)+n(t) (3)

where dis (t) is the distance value measured at time t, d_real(t) is the true distance at time t, n (t) is the mean 0, and the variance σ is satisfied²(ii) a gaussian random variable;

step four, assuming that the number of the base stations arranged in the space is n, and the ith base station coordinate is (x)_i，y_i) Four coordinate points are defined in space, and are respectively expressed as (x)_a，y_a)，(x_b，y_b)，(x_c，y_c)， (x_d，y_d) And satisfy y_a＝y_b，x_b＝x_c，y_c＝y_d，x_a＝x_dNamely, four coordinate points are connected into a rectangle;

step five, respectively placing the four moving targets at the four calibration coordinate points, and linearly moving to the next calibration point in the counterclockwise direction, namely the target A moves from (x)_a，y_a) The point moves to (x)_b，y_b) Point, target B from (x)_b，y_b) The point moves to (x)_c，y_c) Point, target C from (x)_c，y_c) The point moves to (x)_d，y_d) Point, target D from (x)_d，y_d) The point moves to (x)_a，y_a) Point, and the motion speeds of the four targets are uniform; in the process of target movement, the measured distance information d between each moving target and all base stations is obtained in each sampling period_M，i，kWherein d is_M，i，kRepresenting the measured distance of the target M from the ith base station at the time k;

step six, preliminarily estimating the position of each base station, and using fourThe target measures the distance d to each base station at the initial point_A，i，0，d_B，i，0，d_C，i，0，d_D，i，0The position information of each base station is preliminarily solved by using a least square method, and the distance formula equation of the ith base station and the four targets is as follows:

subtracting the first three equations of the equation (4) from the fourth equation, and obtaining the following equations:

A₀Y_i＝B₀ (5)

wherein the content of the first and second substances,

Y_i＝[x_i，y_i]^Twherein [. X [ ]]^TRepresentation matrix [. X [ ]]The transpose matrix of (a) is,

solving for Y using least squares_iThe solving formula is as follows:

preliminarily obtaining an initial point (x) of each base station according to equation (6)_i，0，y_i，0)；

Step seven, defining the target state vector as X ═ X, y, v_x，v_y]^TWhere x and y represent the position coordinates of the object, v_xAnd v_yRepresenting the velocity of the target in the x-direction and the y-direction,

the state vectors of the four targets are defined as X respectively_A，X_B，X_C，X_D，

By adopting a first-step extended Kalman filtering algorithm, the prediction formula is as follows:

X_M，k|k-1＝A_MX_M，k-1|k-1 (7)

wherein M ═ A, B, C or D, each represent four targets, X_M，k-1|k-1Denotes the state of the target M at time k-1, X_M，k|k-1＝[x_M，k|k-1，y_M，k|k-1，v_{x，M，k|k-1}，v_{y，M，k|k-1}]^TFor predicting the state vector of the stage target, P_M，k-1|k-1Representing the state error covariance, P, of the target M at time k-1_M，k|k-1A covariance matrix representing the prediction phase, Q represents the process covariance matrix, set as a diagonal matrix, and when it is the first time (x)_i，y_i) Is taken as the initial point (x) of the base station_i，0，y_i，0)，h_M(X_M，k|k-1) Representing the prediction distance vector between the target M and each base station in the prediction stage;

when M ═ A or C, the matrix A_MAnd H_MSatisfies the following conditions:

when M ═ B or D, the matrix A_MAnd H_MSatisfies the following conditions:

wherein H_MIs h_M(X_M，k|k-1) Jacobian matrix of h_M，i(X_M，k|k-1) Represents h_M(X_M，k|k-1) The ith element of the vector, Δ t being the sampling period;

the kalman gain formula is as follows:

wherein R is a measurement covariance matrix, set as a diagonal matrix,

the correction formula is as follows:

X_M，k|k＝X_M，k|k-1+K_M，k(m_M，k-h_M) (11)

P_M，k|k＝P_M，k|k-1-K_M，kH_MP_M，k|k-1 (12)

wherein m is_M，kA column vector, X, formed by distance measurements of the target M from all base stations at time k_M，k|kFor the state vector of the target M after correction, P_M，k|kFor the corrected error covariance matrix, h_MRepresenting the prediction distance vector between the target M and each base station in the prediction stage;

step eight, obtaining the position information of the targets A, B, C and D according to the step seven, (x)_A，k，y_A，k)，(x_B，k，y_B，k)，(x_C，k，y_C，k)，(x_D，k，y_D，k)，

Adopting a second step of extended Kalman filtering algorithm,

X_i，k|k-1＝A_iX_i，k-1|k-1 (13)

wherein the content of the first and second substances,

X_i，k-1|k-1denotes the state of base station i at time k-1, X_i，k|k-1＝[x_i，k|k-1，y_i，k|k-1，v_{x，i，k|k-1}，v_{y，i，k|k-1}]^TThe state vector, P, of base station i representing the prediction phase_i，k-1|k-1Representing the state error covariance, P, of base station i at time k-1_i，k|k-1Represents the covariance matrix of base station i in the prediction phase, Q' represents the process covariance matrix, set as diagonal matrix, h_i(X_i，k|k-1) A column vector consisting of the distances between the predicted position of base station i and the four target points,

wherein H_iIs h_i(X_i，k|k-1) Jacobian matrix of h_i，p(X_i，k|k-1) Is h_i(X_i，k|k-1) The p-th element of the column vector;

the kalman gain formula is as follows:

wherein R' is a measured covariance matrix, set as a diagonal matrix,

the correction formula is as follows:

X_i，k|k＝X_i，k|k-1+K_i，k(m_i，k-h_i) (18)

P_i，k|k＝P_i，k|k-1-K_i，kH_iP_i，k|k-1 (19)

wherein m is_i，kA column vector, X, formed by distance measurements of base station i at time k from all targets_i，k|kFor the modified state vector of base station i, P_i，k|kFor the corrected error covariance matrix, h_iRepresenting a vector formed by the prediction distances between the ith base station and A, B, C, D four targets respectively in the prediction stage;

and step nine, at each sampling moment, iterating by using the step seven and the step eight until the four target points reach the corresponding end points, and when the four target points reach the target points, the position information of the state vector of each base station is the coordinate position information of the optimized base station.

The coordinate position calibration method of the indoor environment base station based on the Kalman filtering algorithm adopts the technical scheme, namely the method adopts the wireless sensor network to construct an indoor positioning system, all base stations are arranged on the same horizontal plane, a positioned target is positioned to be high with the horizontal plane, a tag is arranged on a mobile robot, and the horizontal distance between the tag and the base stations is solved by adopting the geometric relation of a right triangle; all base stations and the tags are provided with synchronous clocks, the base stations send wireless signals containing timestamps of sending time to the periphery, and when the tags receive the wireless signals, the distances between the tags and the base stations are calculated through time differences; describing noise of distance information between the label and the base station, setting the number and coordinates of the base stations arranged in space, defining four coordinate points which are connected into a rectangle in the space, enabling the moving target to move along the coordinate points, and acquiring the measured distance information between the moving target and all the base stations in each sampling period; measuring the distance to each base station at an initial point by using a moving target, and preliminarily estimating the position of each base station; and obtaining the coordinate position information of the optimized base station through a two-step extended Kalman filtering algorithm. The method overcomes the defects of the traditional positioning method, can accurately finish the base station position calibration in the indoor complex environment, has strong anti-interference capability and effectively improves the positioning precision.

Drawings

The invention is described in further detail below with reference to the following figures and embodiments:

FIG. 1 is a schematic diagram of four coordinate points connected into a rectangle in the fourth step of the method.

Detailed Description

The invention relates to a method for calibrating the coordinate position of an indoor environment base station based on a Kalman filtering algorithm, which comprises the following steps:

step one, setting an indoor positioning system based on a wireless sensor network, wherein single target positioning is composed of a label and a plurality of base stations, the label and the base stations adopt ultra-wideband modules to realize ultra-wideband information transmission, all the base stations are installed on the same horizontal plane, a positioned target is positioned at a height with the horizontal plane, the label is installed on a mobile robot, and the horizontal distance between the label and the base stations is solved by adopting the geometric relation of a right triangle:

wherein d is the distance between the label and the base station, Δ h is the height difference between the label and the base station, d_{Level of}Is the horizontal distance of the tag from the base station;

step two, ranging between each base station and each label adopts a TOF principle, synchronous clocks are set for all the base stations and the labels, the base stations send wireless signals to the periphery, the wireless signals comprise timestamps of sending time, when the labels receive the wireless signals, the tags compare the timestamps of the labels with the timestamps of the received wireless information, and the distance between the labels and the base stations is calculated through time difference:

d＝c·(t_receive-t_send) (2)

where d is the distance between the tag and the base station, c is the propagation speed of light in vacuum, t_receiveTime of reception of radio signal, t_sendTime of transmitting the wireless signal;

step three, noise exists in the distance information between the label and the base station, and the description is as follows:

dis(t)＝d_real(t)+n(t) (3)

where dis (t) is the distance value measured at time t, d_real(t) is the true distance at time t, n (t) is the mean 0, and the variance σ is satisfied²(ii) a gaussian random variable;

step four, assuming that the number of the base stations arranged in the space is n, and the ith base station coordinate is (x)_i，y_i) Four coordinate points are defined in space, and are respectively expressed as (x)_a，y_a)，(x_b，y_b)，(x_c，y_c)， (x_d，y_d) And satisfy y_a＝y_b，x_b＝x_c，y_c＝y_d，x_a＝x_dNamely, four coordinate points are connected into a rectangle;

step five, respectively placing the four moving targets at the four calibration coordinate points, and linearly moving to the next calibration point in the counterclockwise direction, namely the target A moves from (x)_a，y_a) The point moves to (x)_b，y_b) Point, target B from (x)_b，y_b) The point moves to (x)_c，y_c) Point, target C from (x)_c，y_c) The point moves to (x)_d，y_d) Point, target D from (x)_d，y_d) The point moves to (x)_a，y_a) Point, and the motion speeds of the four targets are uniform; in the process of target movement, the measured distance information d between each moving target and all base stations is obtained in each sampling period_M，i，kWherein d is_M，i，kRepresenting the measured distance of the target M from the ith base station at the time k;

step six, preliminarily estimating the position of each base station, and measuring the distance d between each base station and four targets at the initial point respectively_A，i，0，d_B，i，0，d_C，i，0，d_D，i，0The position information of each base station is preliminarily solved by using a least square method, and the distance formula equation of the ith base station and the four targets is as follows:

subtracting the first three equations of the equation (4) from the fourth equation, and obtaining the following equations:

A₀Y_i＝B₀ (5)

wherein the content of the first and second substances,

Y_i＝[x_i，y_i]^Twherein, in the step (A),[*]^Trepresentation matrix [. X [ ]]The transposed matrix of (2). ,

solving for Y using least squares_iThe solving formula is as follows:

the initial point (x) of each base station is preliminarily obtained from equation (56)_i，0，y_i，0)；

Step seven, defining the target state vector as X ═ X, y, v_x，v_y]^TWhere x and y represent the position coordinates of the object, v_xAnd v_yRepresenting the velocity of the target in the x-direction and the y-direction,

the state vectors of the four targets are defined as X respectively_A，X_B，X_C，X_D，

By adopting a first-step extended Kalman filtering algorithm, the prediction formula is as follows:

X_M，k|k-1＝A_MX_M，k-1|k-1 (7)

wherein M ═ A, B, C or D, each represent four targets, X_M，k-1|k-1Denotes the state of the target M at time k-1, X_M，k|k-1＝[x_M，k|k-1，y_M，k|k-1，v_{x，M，k|k-1}，v_{y，M，k|k-1}]^TFor predicting the state vector of the stage target, P_M，k-1|k-1Representing the state error covariance, P, of the target M at time k-1_M，k|k-1A covariance matrix representing the prediction phase, Q represents a process covariance matrix, set as a diagonal matrix, (x) a_i，y_i) Is the location information of the ith base station, when it is the firstAt any moment, (x)_i，y_i) Is taken as the initial point (x) of the base station_i，0，y_i，0)，h_M(X_M，k|k-1) Representing the prediction distance vector between the target M and each base station in the prediction stage;

when M ═ A or C, the matrix A_MAnd H_MSatisfies the following conditions:

when M ═ B or D, the matrix A_MAnd H_MSatisfies the following conditions:

wherein H_MIs h_M(X_M，k|k-1) Jacobian matrix of h_M，i(X_M，k|k-1) Represents h_M(X_M，k|k-1) The ith element of the vector, Δ t being the sampling period;

the kalman gain formula is as follows:

wherein R is a measurement covariance matrix, set as a diagonal matrix,

the correction formula is as follows:

X_M，k|k＝X_M，k|k-1+K_M，k(m_M，k-h_M) (11)

P_M，k|k＝P_M，k|k-1-K_M，kH_MP_M，k|k-1 (12)

wherein m is_M，kA column vector, X, formed by distance measurements of the target M from all base stations at time k_M，k|kFor the state vector of the target M after correction, P_M，k|kFor the corrected error covariance matrix, h_MA prediction distance vector representing the target M and each base station in the prediction stage；

Step eight, obtaining the position information of the targets A, B, C and D according to the step seven, (x)_A，k，y_A，k)，(x_B，k，y_B，k)，(x_C，k，y_C，k)，(x_D，k，y_D，k)，

Adopting a second step of extended Kalman filtering algorithm,

X_i，k|k-1＝A_iX_i，k-1|k-1 〔13)

wherein the content of the first and second substances,

X_i，k-1|k-1denotes the state of base station i at time k-1, X_i，k|k-1＝[x_i，k|k-1，y_i，k|k-1，v_{x，i，k|k-1}，v_{y，i，k|k-1}]^TThe state vector, P, of base station i representing the prediction phase_i，k-1|k-1Representing the state error covariance, P, of base station i at time k-1_i，k|k-1Represents the covariance matrix of base station i in the prediction phase, Q' represents the process covariance matrix, set as diagonal matrix, h_i(X_i，k|k-1) A column vector consisting of the distances between the predicted position of base station i and the four target points,

wherein H_iIs h_i(X_i，k|k-1) Jacobian matrix of h_i，p(X_i，k|k-1) Is h_i(X_i，k|k-1) The p-th element of the column vector;

the kalman gain formula is as follows:

wherein R' is a measured covariance matrix, set as a diagonal matrix,

the correction formula is as follows:

X_i，k|k＝X_i，k|k-1+K_i，k(m_i，k-h_i) (18)

P_i，k|k＝P_i，k|k-1-K_i，kH_iP_i，k|k-1 (19)

wherein m is_i，kA column vector, X, formed by distance measurements of base station i at time k from all targets_i，k|kFor the modified state vector of base station i, P_i，k|kFor the corrected error covariance matrix, h_iRepresenting a vector formed by the prediction distances between the ith base station and A, B, C, D four targets respectively in the prediction stage;

and step nine, at each sampling moment, iterating by using the step seven and the step eight until the four target points reach the corresponding end points, and when the four target points reach the target points, the position information of the state vector of each base station is the coordinate position information of the optimized base station.

The method realizes the calibration of the coordinate position of the base station in the indoor environment by using the extended Kalman filtering algorithm, and the principle of the extended Kalman filtering algorithm is described as follows:

constructing a prediction model:

and (3) calculation in a prediction stage:

X_k|k-1＝AX_k-1|k-1

P_k|k-1＝AP_k-1|k-1A^T+Q

wherein, X_k-1|k-1For time-of-k-1 coordinate informationThe best estimate is made of the estimated values of,

X＝[x y v_x v_y]^T，P_k|k-1a prediction error covariance matrix is obtained, and Q is a process noise covariance matrix;

and (3) calculating in a correction stage:

X_k|k＝X_k|k-1+K_k(m_k-h_k)

P_k|k＝P_k|k-1-K_kH_kP_k|k-1

wherein R is a measurement covariance matrix, m_kFor measured distance vectors, P_k|kTo correct the error covariance matrix, X_k|kI.e. the best estimate of the coordinates at time k.

The method considers the noise interference of the sensor network in the ranging process, adopts the ultra-wideband transmission technology for communication between the base station and the tag, has the characteristics of strong anti-interference performance, high transmission rate, large system capacity, small transmission power, high precision and the like, solves the problem of large difference between the obtained distance information and the actual distance information by using the extended Kalman filtering algorithm, and overcomes the defect of large deviation of the positioning information, thereby ensuring the positioning precision and perfecting the positioning and navigation functions of the robot.

Claims (1)

1. A method for calibrating the coordinate position of an indoor environment base station based on a Kalman filtering algorithm is characterized by comprising the following steps:

step one, setting an indoor positioning system based on a wireless sensor network, wherein single target positioning is composed of a label and a plurality of base stations, the label and the base stations adopt ultra-wideband modules to realize ultra-wideband information transmission, all the base stations are installed on the same horizontal plane, a positioned target is positioned at a height with the horizontal plane, the label is installed on a mobile robot, and the horizontal distance between the label and the base stations is solved by adopting the geometric relation of a right triangle:

wherein d is the distance between the label and the base station, Δ h is the height difference between the label and the base station, d_{Level of}Is the horizontal distance of the tag from the base station;

step two, ranging between each base station and each label adopts a TOF principle, synchronous clocks are set for all the base stations and the labels, the base stations send wireless signals to the periphery, the wireless signals comprise timestamps of sending time, when the labels receive the wireless signals, the tags compare the timestamps of the labels with the timestamps of the received wireless information, and the distance between the labels and the base stations is calculated through time difference:

d＝c·(t_receive-t_send) (2)

where d is the distance between the tag and the base station, c is the propagation speed of light in vacuum, t_receiveTime of reception of radio signal, t_sendTime of transmitting the wireless signal;

step three, noise exists in the distance information between the label and the base station, and the description is as follows:

dis(t)＝d_real(t)+n(t) (3)

where dis (t) is the distance value measured at time t, d_real(t) is the true distance at time t, n (t) is the mean 0, and the variance σ is satisfied²(ii) a gaussian random variable;

step four,Suppose that the number of base stations arranged in space is n, and the ith base station coordinate is (x)_i，y_i) Four coordinate points are defined in space, and are respectively expressed as (x)_a，y_a)，(x_b，y_b)，(x_c，y_c)，(x_d，y_d) And satisfy y_a＝y_b，x_b＝x_c，y_c＝y_d，x_a＝x_dNamely, four coordinate points are connected into a rectangle;

step five, respectively placing the four moving targets at the four calibration coordinate points, and linearly moving to the next calibration point in the counterclockwise direction, namely the target A moves from (x)_a，y_a) The point moves to (x)_b，y_b) Point, target B from (x)_b，y_b) The point moves to (x)_c，y_c) Point, target C from (x)_c，y_c) The point moves to (x)_d，y_d) Point, target D from (x)_d，y_d) The point moves to (x)_a，y_a) Point, and the motion speeds of the four targets are uniform; in the process of target movement, the measured distance information d between each moving target and all base stations is obtained in each sampling period_M，i，kWherein d is_M，i，kRepresenting the measured distance of the target M from the ith base station at the time k;

step six, preliminarily estimating the position of each base station, and measuring the distance d between each base station and four targets at the initial point respectively_A，j，0，d_B，i，0，d_c，i，0，d_D，i，0The position information of each base station is preliminarily solved by using a least square method, and the distance formula equation of the ith base station and the four targets is as follows:

subtracting the first three equations of the equation (4) from the fourth equation, and obtaining the following equations:

A₀Y_i＝B₀ (5)

wherein the content of the first and second substances,

Y_i＝[x_i，y_i]^Twherein [. X [ ]]^TRepresentation matrix [. X [ ]]The transpose matrix of (a) is,

solving for Y using least squares_iThe solving formula is as follows:

preliminarily obtaining an initial point (x) of each base station according to equation (6)_i，0，y_i，0)；

Step seven, defining the target state vector as X ═ X, y, v_x，v_y]^TWhere x and y represent the position coordinates of the object, v_xAnd v_yRepresenting the velocity of the target in the x-direction and the y-direction,

the state vectors of the four targets are defined as X respectively_A，X_B，X_C，X_D，

By adopting a first-step extended Kalman filtering algorithm, the prediction formula is as follows:

X_M，k|k-1＝A_MX_M，k-1|k-1 (7)

wherein M ═ A, B, C or D, each represent four targets, X_M，k-1|k-1Denotes the state of the target M at time k-1, X_M，k|k-1＝[x_M，k|k-1，y_M，k|k-1，v_{x，M，k|k-1}，v_{y，M，k|k-1}]^TFor predicting the state vector of the stage target, P_M，k-1|k-1Representing the state error covariance, P, of the target M at time k-1_M，k|k-1A covariance matrix representing the prediction phase, Q represents the process covariance matrix, set as a diagonal matrix, and when it is the first time (x)_i，y_i) Is taken as the initial point (x) of the base station_i，0，y_i，0)，h_M(X_M，k|k-1) Representing the prediction distance vector between the target M and each base station in the prediction stage;

when M ═ A or C, the matrix A_MAnd H_MSatisfies the following conditions:

when M ═ B or D, the matrix A_MAnd H_MSatisfies the following conditions:

wherein H_MIs h_M(X_M，k|k-1) Jacobian matrix of h_M，i(X_M，k|k-1) Represents h_M(X_M，k|k-1) The ith element of the vector, Δ t being the sampling period;

the kalman gain formula is as follows:

wherein R is a measurement covariance matrix, set as a diagonal matrix,

the correction formula is as follows:

X_M，k|k＝X_M，k|k-1+K_M，k(m_M，k-h_M) (11)

P_M，k|k＝P_M，k|k-1-K_M，kH_MP_M，k|k-1 (12)

wherein m is_M，kA column vector, X, formed by distance measurements of the target M from all base stations at time k_M，k|kFor the state vector of the target M after correction, P_M，k|kFor the corrected error covariance matrix, h_MRepresenting the prediction distance vector between the target M and each base station in the prediction stage;

step eight, obtaining the position information of the targets A, B, C and D according to the step seven, (x)_A，k，y_A，k)，(x_B，k，y_B，k)，(x_C，k，y_C，k)，(x_D，k，y_D，k)，

Adopting a second step of extended Kalman filtering algorithm,

X_i，k|k-1＝A_iX_i，k-1|k-1 (13)

wherein the content of the first and second substances,

X_i，k-1|k-1denotes the state of base station i at time k-1, X_i，k|k-1＝[x_i，k|k-1，y_i，k|k-1，v_{x，i，k|k-1}，v_{y，i，k|k-1}]^TThe state vector, P, of base station i representing the prediction phase_i，k-1|k-1Representing the state error covariance, P, of base station i at time k-1_i，k|k-1Represents the covariance matrix of base station i in the prediction phase, Q' represents the process covariance matrix, set as diagonal matrix, h_i(X_i，k|k-1) A column vector consisting of the distances between the predicted position of base station i and the four target points,

wherein H_iIs h_i(X_i，k|k-1) Jacobian matrix of h_i，p(X_i，k|k-1) Is h_i(X_i，k|k-1) The p-th element of the column vector;

the kalman gain formula is as follows:

wherein R' is a measured covariance matrix, set as a diagonal matrix,

the correction formula is as follows:

X_i，k|k＝X_i，k|k-1+K_i，k(m_i，k-h_i) (18)

P_i，k|k＝P_i，k|k-1-K_i，kH_iP_i，k|k-1 (19)

wherein m is_i，kA column vector, X, formed by distance measurements of base station i at time k from all targets_i，k|kFor the modified state vector of base station i, P_i，k|kFor the corrected error covariance matrix, h_iRepresenting a vector formed by the prediction distances between the ith base station and A, B, C, D four targets respectively in the prediction stage;

and step nine, at each sampling moment, iterating by using the step seven and the step eight until the four target points reach the corresponding end points, and when the four target points reach the target points, the position information of the state vector of each base station is the coordinate position information of the optimized base station.

Images