Bluetooth and sound signal fusion positioning method for patrol inspection of indoor unmanned aerial vehicle of transformer substation

Abstract

The invention discloses a Bluetooth and sound signal fusion positioning method for transformer substation indoor unmanned aerial vehicle inspection, and belongs to the technical field of indoor positioning. Comprising the following steps: deploying an unmanned aerial vehicle positioning system based on sound signals and Bluetooth; performing sound signal quality evaluation on the received sound signal and a reference signal; calculating the time difference of arrival of the complete set sound signal and estimating the position; sound signal positioning quality evaluation; bluetooth signal receiving and multilateral positioning are carried out; bluetooth linear positioning constraint is applied; performing multi-source data fusion positioning; and constructing and updating a Bluetooth fingerprint database. According to the method, the Bluetooth fingerprint database is updated in real time by using sound signal positioning, Bluetooth positioning is optimized, and Bluetooth and sound signal positioning data are fused, so that indoor high-precision real-time positioning of the unmanned aerial vehicle is realized, and the problem of satellite positioning failure in an indoor complex environment of a transformer substation is effectively solved; and meanwhile, high-precision indoor positioning of the unmanned aerial vehicle is realized, the coverage range in a complex scene in a substation is expanded, and the robustness and reliability of routing inspection and positioning of the unmanned aerial vehicle are improved.

Description

Technical Field

[0001] The present invention belongs to the field of indoor positioning technology, and in particular relates to a Bluetooth and acoustic signal fusion positioning method for indoor unmanned aerial vehicle inspection of a substation. Background Art

[0002] With the advancement of intelligent substations, the application of drone technology in indoor and outdoor inspections has become a trend. With its advantages of small size, light weight, high flexibility, wide viewing angle and high efficiency, drones have been widely used in main distribution network lines and outdoor substation inspections. However, indoor inspection tasks still face many technical bottlenecks and challenges, which affect the application effect of drones and the reliability of task execution. First, the indoor environment of substations is complex. High-density metal structures and concrete walls form a satellite denial environment, which leads to the failure of satellite positioning technologies such as Beidou and GPS, and drones cannot achieve accurate positioning through satellite navigation technology. Secondly, existing indoor positioning technologies, such as lidar, binocular vision, UWB, WI-FI, etc., are usually based on a single signal source. Although positioning is achieved to a certain extent, they generally have disadvantages such as high cost, poor environmental adaptability, insufficient accuracy, high power consumption and sensitivity to light. In particular, they perform poorly in strong electromagnetic interference, low illumination and weak texture environments in substations, making it difficult to achieve accurate positioning. Therefore, indoor inspections of substations urgently need a new fusion positioning solution to provide high-precision real-time positioning to ensure stable flight and accurate inspection of drones in complex power environments. Summary of the invention

[0003] The purpose of the present invention is to provide a Bluetooth and acoustic signal fusion positioning method for indoor UAV inspection of substations to solve the problems raised in the above background technology.

[0004] The object of the present invention is achieved by: a Bluetooth and acoustic signal fusion positioning method for indoor UAV inspection of substations, characterized in that the method comprises the following steps:

[0005] Step S1: deploying a drone positioning system based on acoustic signals and Bluetooth;

[0006] Step S2: receiving an acoustic signal, and performing acoustic signal quality evaluation on the received acoustic signal and a reference signal;

[0007] Step S3: Calculate the arrival time difference of all sound signals and perform position estimation;

[0008] Step S4: evaluating the acoustic signal localization quality;

[0009] Step S5: Bluetooth signal reception and multi-lateration positioning;

[0010] Step S6: applying Bluetooth linear positioning constraints;

[0011] Step S7: Multi-source data fusion positioning.

[0012] Preferably, the deployment of a drone positioning system based on acoustic signals and Bluetooth in step S1 is specifically as follows:

[0013] The drone positioning system includes a drone terminal, a Bluetooth beacon, an acoustic signal base station and a central positioning server, wherein N acoustic signal base stations are deployed at the four corners of the ceiling edge to emit predefined acoustic signals with synchronous delay;

[0014] The Bluetooth beacon is placed on the ceiling above the area to be located so that the Bluetooth messages it sends cover the entire target location area;

[0015] The drone terminal is equipped with a microphone sensor and a Bluetooth module, the onboard microphone receives a frequency-modulated sound signal from an acoustic signal base station, and the onboard Bluetooth module receives a BLE message with RSSI transmitted by a Bluetooth beacon;

[0016] The Bluetooth beacon, acoustic base station, drone terminal, and central positioning server that performs positioning communicate with each other.

[0017] Preferably, in step S2, the acoustic signal is received, and the received acoustic signal and the reference signal are subjected to acoustic signal quality evaluation, specifically:

[0018] Step S2-1: Signal strength evaluation:

[0019] The signal strength evaluation S is calculated based on the power of the received signal and is expressed by the following formula:

[0020]

[0021] Among them, P r is the received power of the acoustic signal, P ref is the received power of the reference signal;

[0022] Step S2-2: Signal-to-noise ratio evaluation:

[0023] The signal-to-noise ratio is used to evaluate the relative strength of the received acoustic signal and noise and is calculated using the following formula:

[0024]

[0025] Among them, P noi is the power of background noise;

[0026] Step S2-3: Cross-correlation coefficient evaluation:

[0027] The cross-correlation coefficient evaluates the similarity between the received signal and the reference signal. The cross-correlation function R between the two signals xy (τ) is expressed as:

[0028]

[0029] Where x(t) is the reference signal, y(t) is the received signal, and τ is the signal delay;

[0030] Step S2-4: construct a unified acoustic signal quality evaluation standard.

[0031] Preferably, in step S2-4, a unified acoustic signal quality evaluation standard is constructed, specifically:

[0032] Step S2-4-1: setting the three signal quality evaluation indicators of signal strength, signal-to-noise ratio and correlation between signals as the criterion layer, and setting the comprehensive signal quality evaluation as the target layer;

[0033] The relative importance of these dimensions is evaluated through expert scoring, and converted into a scoring scale of 1-9 to construct a judgment matrix, where each element in the matrix represents the importance of the i-th dimension relative to the j-th dimension, as follows:

[0034]

[0035] Among them, p S,SNR Indicates the importance of signal strength relative to the signal-to-noise ratio, p S,R Indicates the importance of signal strength relative to the cross-correlation coefficient; p SNR,R is the importance of the signal-to-noise ratio relative to the cross-correlation coefficient;

[0036] Normalize the judgment matrix to obtain the weight of each signal evaluation dimension, that is, the signal strength weight w S , signal-to-noise ratio weight w SNR , the correlation weight w between signals R ;

[0037] Step S2-4-2: Perform consistency check on the matrix, that is, calculate the consistency ratio CR:

[0038] Calculate the maximum eigenvalue λ of the judgment matrix max , and calculate its corresponding consistency index CI:

[0039]

[0040] Where z is the dimension of the matrix;

[0041] Introducing the random consistency index RI, for a judgment matrix with a dimension of 3, RI is 0.58. At this time,

[0042]

[0043] If CR is less than a certain value, the consistency of the judgment matrix is ​​acceptable;

[0044] Step S2-4-3: Construct a comprehensive signal quality evaluation formula using the weights obtained from steps S2-4-1 to S2-4-2:

[0045] U=w S ·S norm +w SNR SNR norm +w ρ ·ρ norm +w CIR ·CIR norm ;

[0046] According to the comprehensive score U, set the threshold U th Determine the quality of the received signal; if the comprehensive score U exceeds the threshold U th , the signal quality is considered to meet the requirements and is suitable for position estimation; otherwise, the received signal is considered to not meet the requirements.

[0047] Preferably, the time difference of arrival of all collected sound signals in step S3 is calculated and the position is estimated, specifically:

[0048] Step S3-1: Calculation of the full set of TDOA measurements:

[0049] The UAV positioning system based on acoustic signals includes M acoustic signal base stations with known positions, which are s i , i=1,2,…,M;

[0050] For acoustic signal base station i and acoustic signal base station j, the TDOA measurement value r between them ij Defined as:

[0051] r ij =∥us i ∥-∥us j ∥+n ij +o ij +η ij ;

[0052] Where u is the unknown position vector of the signal source; s i and j are the position vectors of acoustic signal base station i and acoustic signal base station j respectively; n ij is the line-of-sight measurement noise, which obeys zero-mean Gaussian distribution; o ij is the abnormal data error, caused by measurement noise or equipment error; η ij is the deviation of the non-line-of-sight path, which is a positive value;

[0053] The TDOA measurements of all acoustic signal base station pairs are constructed into a matrix R:

[0054]

[0055] Among them, r ij represents the TDOA value from sensor i to sensor j;

[0056] Step S3-2: Identify NLOS measurements using cyclic redundancy detection based on geometric constraints:

[0057] Using the geometric constraint characteristics of the full set TDOA matrix, the outliers are judged by the cyclic sum of the closed path. For the closed path of any acoustic signal base station, its TDOA measurement value satisfies the following cyclic sum condition:

[0058] r 12 +r 23 +r 31 ≈0;

[0059] If the cycle sum of a path does not satisfy this condition, that is, |r 12 +r 23 +r 31 |>∈, indicating that there is a non-line-of-sight signal on this path;

[0060] Ideally, the full set TDOA matrix R should satisfy the rank constraint. When there is only LOS signal, the rank of the matrix should be 2. If the rank of R is greater than 2, it means that the matrix contains abnormal data or NLOS path.

[0061] Step S3-3: NLOS detection and removal algorithm:

[0062] Based on the cyclic sum and rank constraints, a set of iterative steps are defined to identify and remove NLOS measurements, as follows:

[0063] Define the initialization loss function L to evaluate the consistency of the measured data:

[0064] L=∑ (i,j)∈S (r ij -(∥Γ-s i ∥-∥Γ-s j ∥)) 2 ;

[0065] Among them, S is the set of base station pairs for TDOA measurement, Γ is the estimated value of the source position, and the optimal estimate is obtained by minimizing the loss function; in the process of minimizing the loss function, the TDOA measurement values ​​of each pair of base station pairs are traversed one by one to check whether the loop and condition are met:

[0066] |r ij +r jk +r ki |>∈;

[0067] For paths that do not meet this condition, the TDOA measurement values ​​with large errors are marked as NLOS values ​​and removed from the set S. After removing the NLOS measurement values, the set S is updated, the source position estimate u is recalculated, and the process is iterated until all closed paths meet the loop and condition or the loss function is no longer significantly reduced.

[0068] Step S3-4: Final position estimation:

[0069] After removing the NLOS measurements, the final source position is estimated by minimizing the updated loss function:

[0070]

[0071] Among them, S LOS is the line-of-sight (LOS) measurement set after NLOS removal.

[0072] Preferably, the acoustic signal localization quality is evaluated in step S4, specifically:

[0073] Step S4-1: Calculate the mean of the current positioning samples:

[0074] The sample mean represents the central tendency of multiple measurement results and indicates the optimal estimate of the coordinate position. The expression is as follows:

[0075]

[0076] in, and The current position is the mean value of the position estimation under n measurements, x i and i are the x and y coordinates of the ith measurement respectively;

[0077] Step S4-2: Calculate the standard deviation of the current positioning sample:

[0078] The sample standard deviation of the positioning coordinates measures the degree of dispersion of multiple measurement results and reflects the distribution of positioning data around the mean. The larger the standard deviation, the more dispersed the measurement results and the greater the uncertainty of positioning. The calculation process is as follows:

[0079]

[0080] Among them, s x and y They are the standard deviations of the x and y coordinates of n measurements updated in real time;

[0081] Step S4-3: Calculate the confidence interval of the current positioning point:

[0082] Based on the sample mean, standard deviation, and t value, calculate the confidence interval at the current coordinates:

[0083]

[0084] Among them, CI x and CI y are the confidence intervals for the x and y coordinates, respectively; the t value is 2.262;

[0085] The width of the confidence interval of the position estimate indicates the degree of fluctuation of the two-dimensional positioning estimate at that position, as shown in the following formula:

[0086]

[0087] Among them, d wx and d wy Represents the width of the confidence interval at the x and y coordinates at the current confidence level.

[0088] Preferably, the Bluetooth signal reception and multilateral positioning in step S5 are specifically as follows:

[0089] Step S5-1: Bluetooth RSSI signal reception and distance estimation:

[0090] In the drone positioning system, the relationship between RSSI value and distance is expressed using the path loss model:

[0091] RSSI=-(10clog 10 (d)+A);

[0092] Where RSSI is the received signal strength in dBm; d is the distance between the device and the Bluetooth beacon; A is the signal strength value measured at a reference distance of 1 meter; c is the path loss index;

[0093] RSSI = -(10nlog 10 (d)+A) is transformed to derive the estimated distance d between the device and the Bluetooth beacon:

[0094]

[0095] Step S5-2: Bluetooth beacon multilateration positioning:

[0096] Assume the coordinates of Bluetooth beacon i are (x i ,y i ), the position coordinates of the drone to be located are (x, y), and the distance between it and beacon i is d i , then the distance relationship between the drone and the beacon is:

[0097]

[0098] Using the distance d between multiple beacons in space and the drone i Construct multiple nonlinear equations and simplify them into linear equations through elimination method, that is, solve the position coordinates of the UAV.

[0099] Preferably, applying Bluetooth linear positioning constraints in step S6 is specifically:

[0100] The likelihood function derived from the RSSI distance model is used to calculate the distance error from the device to the beacon;

[0101] Create a circular density function around each beacon The radius is determined based on the measured data and all measurements are accumulated and summed. The expression is as follows:

[0102]

[0103] in, To locate the two-dimensional position of the terminal, is the 2D position of the ith beacon, d i is the distance measured by the i-th beacon; the distance d between the drone terminal and the beacon is estimated by measuring the signal strength i , and find a target position x so that the actual distance from the drone to the beacon The distance d from the inferred i The difference between them is the smallest; by minimizing this likelihood function The actual position of the drone can be estimated;

[0104] To further optimize the localization, the position estimate is constrained using the following linear constraints:

[0105]

[0106] Among them, A is the linear constraint matrix, which represents the range of the area to be located;

[0107] Assuming that the area to be located is a rectangle, A and vector b in Ax≤b are expressed as:

[0108]

[0109] Among them, x min and x max Indicates the left and right boundaries of the room, y min and max Indicates the upper and lower boundaries of the room.

[0110] Preferably, the multi-source data fusion positioning in step S7 is specifically as follows:

[0111] Step S7-1: State vector definition

[0112] Set the state vector X of the drone terminal center of mass to include two parts: position p and velocity v:

[0113]

[0114] Step S7-2: Setting the system state transfer equation. The nonlinear state transfer equation f of the system is expressed as:

[0115] X k|k-1 =f(X k-1|k-1 ,u k ,q k );

[0116] Among them, u k is the control input vector, q k is the process noise vector;

[0117] The state transfer of the system follows the kinematic model. The position of the UAV terminal changes with the speed. The state transfer equation describes the change of position and speed, which can be expressed as:

[0118]

[0119] v k =v k-1 +a k-1 Δt;

[0120] Where a is the acceleration and Δt is the time step. Substitute the motion model into the state transfer function:

[0121]

[0122] The corresponding state transfer function f corresponds to the Jacobian matrix F k for:

[0123]

[0124] in, is the partial derivative of the state transfer function f with respect to the state vector X, indicating the degree of local linearization of f with respect to the state; I is the unit matrix, indicating the autocorrelation of the variable;

[0125] Step S7-3: Update the prediction covariance matrix and set the state covariance matrix prediction equation to:

[0126]

[0127] Among them, P is the covariance matrix of the state vector, Q is the covariance matrix of the process noise, and L k is the Jacobian matrix of the noise;

[0128] Step S7-4: Observation model and update equation:

[0129] Considering the fusion of acoustic signals and Bluetooth fingerprint positioning information, the observation equation Z is set k It is the combined information of two sensors:

[0130] Z k =h(X k|k-1 ,r k );

[0131] Among them, h(X) is the observation function, Bluetooth fingerprint and acoustic signal provide different location information, r k is the observation noise;

[0132] The Jacobian matrix H of the observation equation k It is expressed as:

[0133]

[0134] Preferably, the observation update step includes calculating the update of the residual, the innovation covariance, the Kalman gain and the state vector, as follows:

[0135] Calculate the residual and convert the sensor's observation value Z k and the state X predicted by the system k|k-1 Compare and calculate the observed residual Y k :

[0136] Y k =Z k -h(X k|k-1 ,r k );

[0137] Among them, Z k represents the observation data of the sensor, h(X) is the observation function, which represents the observation value estimated by the system from the state vector;

[0138] Calculate the innovation covariance matrix and combine it with the observation noise R k and the state covariance matrix P k|k-1 To calculate the innovation covariance matrix S k :

[0139]

[0140] Calculate the Kalman gain and use the Kalman gain K k Adjust the system's confidence in observed data:

[0141]

[0142] Combined with the predicted state X k|k-1 and the observed residual Y k , using the Kalman gain Kk Update the state vector X of the smart terminal k|k :

[0143] X k|k =X k|k-1 +K k Y k ;

[0144] As observation information is continuously introduced, the value of the covariance matrix will gradually decrease:

[0145] P k|k =(IK k H k ) k|k-1 ;

[0146] Among them, H k Jacobian matrix of the observation equation.

[0147] Compared with the prior art, the present invention has the following improvements and advantages:

[0148] 1. Use acoustic signal positioning to update the Bluetooth fingerprint library in real time, optimize Bluetooth positioning, and integrate Bluetooth and acoustic signal positioning data to achieve high-precision real-time positioning of drones indoors, effectively solving the problem of satellite positioning failure in the complex indoor environment of substations; at the same time, it achieves high-precision indoor positioning of drones, expands its coverage in complex indoor scenes of substations, and improves the positioning robustness and reliability of drone inspections.

[0149] 2. Using drones equipped with Bluetooth and acoustic signal sensors, the acoustic signal position estimation information is introduced in the area where single Bluetooth fingerprint positioning fails, and the Bluetooth fingerprint database is accurately calibrated and updated in real time, effectively overcoming the decrease in positioning accuracy caused by the uneven distribution of Bluetooth fingerprint data.

[0150] 3. By making full use of the complementarity and synergy of multi-source data, this case significantly reduces the misjudgment rate and improves the accuracy and reliability of positioning, economically and efficiently meeting the needs of indoor drone inspections. BRIEF DESCRIPTION OF THE DRAWINGS

[0151] Figure 1 Flow chart of the method of the present invention.

[0152] Figure 2 Schematic diagram of the structure of the UAV positioning system based on acoustic signals and Bluetooth.

[0153] Figure 3 This is a schematic diagram of the Bluetooth RSSI signal strength contour. DETAILED DESCRIPTION

[0154] The present invention is further summarized below with reference to the accompanying drawings.

[0155] like Figure 1 As shown, a Bluetooth and acoustic signal fusion positioning method for indoor UAV inspection of substations includes the following steps:

[0156] Step S1: Deploy a drone positioning system based on acoustic signals and Bluetooth, specifically:

[0157] like Figure 2 As shown in the figure, the UAV positioning system includes a UAV terminal, a Bluetooth beacon, an acoustic signal base station and a central positioning server. N acoustic signal base stations are deployed at the four corners of the ceiling edge to send out predefined acoustic signals with synchronous delays; multiple Bluetooth beacons are placed on the ceiling above the area to be positioned so that the Bluetooth messages they send can cover the entire target positioning area as much as possible; the UAV terminal is equipped with a microphone sensor and a Bluetooth module, wherein the onboard microphone receives the FM acoustic signal from the acoustic signal base station, and the onboard Bluetooth module receives the BLE message with RSSI transmitted by the Bluetooth beacon; the Bluetooth beacon, the acoustic base station, the UAV terminal and the central positioning server that performs positioning communicate with each other.

[0158] In step S2, an acoustic signal is received, and the received acoustic signal and a reference signal are compared to perform acoustic signal quality evaluation, specifically:

[0159] After the deployment of the drone positioning system combining Bluetooth and acoustic signals, the acoustic signal base station sends out synchronous delayed FM acoustic signals, and the microphone on the drone receives these acoustic signals for preliminary positioning. Due to the influence of multipath effects and noise in the indoor area of ​​the substation, the quality of acoustic signals received at different locations is different. Therefore, in order to ensure that the quality of the received acoustic signal meets the requirements of high-precision positioning, the acoustic signal quality is evaluated by comparing the difference between the received acoustic signal and the reference signal.

[0160] Step S2-1: Signal strength evaluation:

[0161] The signal strength evaluation S is calculated based on the power of the received signal and is expressed by the following formula:

[0162]

[0163] Among them, P r is the received power of the acoustic signal, P ref is the received power of the reference signal;

[0164] Step S2-2: Signal-to-noise ratio evaluation:

[0165] The signal-to-noise ratio is used to evaluate the relative strength of the received acoustic signal and noise and is calculated using the following formula:

[0166]

[0167] Among them, P noi is the power of background noise;

[0168] Step S2-3: Cross-correlation coefficient evaluation:

[0169] The cross-correlation coefficient evaluates the similarity between the received signal and the reference signal. The cross-correlation function R between the two signals xy (τ) is expressed as:

[0170]

[0171] Where x(t) is the reference signal, y(t) is the received signal, and τ is the signal delay;

[0172] Step S2-4: construct a unified acoustic signal quality evaluation standard, specifically:

[0173] A unified acoustic signal quality evaluation standard is needed to evaluate the quality of the acoustic signal received by the drone terminal from three dimensions: signal strength, signal-to-noise ratio, and correlation between signals, to determine whether it meets the standard of high-precision positioning signal quality, and thus to estimate the position of the signal that meets the acoustic signal quality evaluation standard.

[0174] Step S2-4-1: setting the three signal quality evaluation indicators of signal strength, signal-to-noise ratio and correlation between signals as the criterion layer, and setting the comprehensive signal quality evaluation as the target layer;

[0175] The relative importance of these dimensions is evaluated through expert scoring, and converted into a scoring scale of 1-9 to construct a judgment matrix, where each element in the matrix represents the importance of the i-th dimension relative to the j-th dimension, as follows:

[0176]

[0177] Among them, p S,SNR Indicates the importance of signal strength relative to the signal-to-noise ratio, p S,R Indicates the importance of signal strength relative to the cross-correlation coefficient; p SNR,R is the importance of the signal-to-noise ratio relative to the cross-correlation coefficient;

[0178] Normalize the judgment matrix to obtain the weight of each signal evaluation dimension, that is, the signal strength weight w S , signal-to-noise ratio weight w SNR , the correlation weight w between signals R ;

[0179] Step S2-4-2: Perform consistency check on the matrix, that is, calculate the consistency ratio CR:

[0180] Calculate the maximum eigenvalue λ of the judgment matrix max, and calculate its corresponding consistency index CI:

[0181]

[0182] Where z is the dimension of the matrix;

[0183] Introducing the random consistency index RI, for a judgment matrix with a dimension of 3, RI is 0.58. At this time,

[0184]

[0185] If CR is less than a certain value, the consistency of the judgment matrix is ​​acceptable;

[0186] Step S2-4-3: Construct a comprehensive signal quality evaluation formula using the weights obtained from steps S2-4-1 to S2-4-2:

[0187] U=w S ·S norm +w SNR SNR norm +w ρ ·ρ norm +w CIR ·CIR norm ;

[0188] According to the comprehensive score U, set the threshold U th Determine the quality of the received signal; if the comprehensive score U exceeds the threshold U th , the signal quality is considered to meet the requirements and is suitable for position estimation; otherwise, the received signal is considered to not meet the requirements.

[0189] In different indoor positioning scenarios, the importance of the four evaluation criteria varies; for example, in a noisy environment, the weight of the signal-to-noise ratio should be increased. SNR When the signal transmission distance is long, the weight of signal strength should be appropriately increased. S By dynamically adjusting the weights, it can more effectively adapt to various environments and requirements in the substation room.

[0190] Step S2 completes the quality evaluation of the real-time received sound signal. Only the sound signal that meets the quality evaluation standard is used to calculate the complete TDOA value of the sound signal set and perform position estimation; Step S3 calculates the arrival time difference of the full set of sound signals and performs position estimation, specifically:

[0191] Step S3-1: Calculation of the full set of TDOA measurements:

[0192] The UAV positioning system based on acoustic signals includes M acoustic signal base stations with known positions, which are s i , i=1,2,…,M;

[0193] For acoustic signal base station i and acoustic signal base station j, the TDOA measurement value r between them ij Defined as:

[0194] r ij =∥us i ∥-∥us j ∥+n ij +o ij +η ij ;

[0195] Where u is the unknown position vector of the signal source; s i and j are the position vectors of acoustic signal base station i and acoustic signal base station j respectively; n ij is the line-of-sight measurement noise, which obeys zero-mean Gaussian distribution; o ij is the abnormal data error, caused by measurement noise or equipment error; η ij is the deviation of the non-line-of-sight path, which is a positive value;

[0196] The TDOA measurements of all acoustic signal base station pairs are constructed into a matrix R:

[0197]

[0198] Among them, r ij represents the TDOA value from sensor i to sensor j;

[0199] Step S3-2: Identify NLOS measurements using cyclic redundancy detection based on geometric constraints:

[0200] In order to identify and remove NLOS measurements, the geometric constraint characteristics of the full set TDOA matrix are used to determine outliers through the cyclic sum of closed paths. For any closed path of an acoustic signal base station, its TDOA measurement value satisfies the following cyclic sum condition:

[0201] r 12 +r 23 +r 31 ≈0;

[0202] If the cycle sum of a path does not satisfy this condition, that is, |r 12 +r 23 +r 31 |>∈, indicating that there is a non-line-of-sight signal on this path;

[0203] Ideally, the full set TDOA matrix R should satisfy the rank constraint. When there is only LOS signal, the rank of the matrix should be 2. If the rank of R is greater than 2, it means that the matrix contains abnormal data or NLOS path.

[0204] Step S3-3: NLOS detection and removal algorithm:

[0205] Based on the cyclic sum and rank constraints, a set of iterative steps are defined to identify and remove NLOS measurements, as follows:

[0206] Define the initialization loss function L to evaluate the consistency of the measured data:

[0207] L=∑ (i,j)∈S (r ij -(∥Γ-s i ∥-∥Γ-s j ∥)) 2 ;

[0208] Among them, S is the set of base station pairs for TDOA measurement, Γ is the estimated value of the source position, and the optimal estimate is obtained by minimizing the loss function; in the process of minimizing the loss function, the TDOA measurement values ​​of each pair of base station pairs are traversed one by one to check whether the loop and condition are met:

[0209] |r ij +r jk +r ki |>∈;

[0210] For paths that do not meet this condition, the TDOA measurement values ​​with large errors are marked as NLOS values ​​and removed from the set S. After removing the NLOS measurement values, the set S is updated, the source position estimate u is recalculated, and the process is iterated until all closed paths meet the loop and condition or the loss function is no longer significantly reduced.

[0211] Step S3-4: Final position estimation:

[0212] After removing the NLOS measurements, the final source position is estimated by minimizing the updated loss function:

[0213]

[0214] Among them, S LOS is the line-of-sight (LOS) measurement set after NLOS removal. Through the above steps, non-line-of-sight measurements can be identified and removed based on geometric characteristics, thereby improving the accuracy and robustness of positioning.

[0215] Step S3 realizes the real-time position estimation of the indoor drone through the acoustic signal, and each measurement result may be different. Step S4 calculates the confidence space of the current positioning coordinates to evaluate the position quality, specifically:

[0216] Step S4-1: Calculate the mean of the current positioning samples:

[0217] The sample mean represents the central tendency of multiple measurement results and indicates the optimal estimate of the coordinate position. The expression is as follows:

[0218]

[0219] in, and The current position is the mean value of the position estimation under n measurements, x i and i are the x and y coordinates of the ith measurement respectively;

[0220] Step S4-2: Calculate the standard deviation of the current positioning sample:

[0221] The sample standard deviation of the positioning coordinates measures the degree of dispersion of multiple measurement results and reflects the distribution of positioning data around the mean. The larger the standard deviation, the more dispersed the measurement results and the greater the uncertainty of positioning. The calculation process is as follows:

[0222]

[0223] Among them, s x and y They are the standard deviations of the x and y coordinates of n measurements updated in real time;

[0224] Step S4-3: Calculate the confidence interval of the current positioning point:

[0225] Based on the sample mean, standard deviation, and t value, calculate the confidence interval at the current coordinates:

[0226]

[0227] Among them, CI x and CI y are the confidence intervals for the x and y coordinates, respectively; the t value is 2.262;

[0228] The width of the confidence interval of the position estimate indicates the degree of fluctuation of the two-dimensional positioning estimate at that position, as shown in the following formula:

[0229]

[0230] Among them, d wx and d wy Respectively represent the confidence interval widths at the x and y coordinates at the current confidence level. The confidence space of the current estimated position is calculated through the above steps, and the confidence interval width threshold T is set according to the system accuracy requirements and actual experience. If the confidence interval width is greater than the threshold T, it means that the acoustic signal positioning fluctuates greatly, and the acoustic positioning is inaccurate at this time, and it needs to be updated and re-positioned; if the confidence interval width is less than the threshold, it is considered that the positioning quality meets the requirements.

[0231] Step S5: Bluetooth signal reception and multi-lateration positioning;

[0232] Step S1 completes the deployment of the drone positioning system based on Bluetooth harmonic signals, where the Bluetooth module carried by the drone receives the RSSI signal emitted by the Bluetooth beacon, that is, the strength of the signal when it arrives; by analyzing the RSSI value, the distance between the drone and each Bluetooth beacon is estimated, and then the known coordinates of each beacon are used to realize the drone positioning based on Bluetooth signals through multilateral measurement; the specific steps are as follows:

[0233] Step S5-1: Bluetooth RSSI signal reception and distance estimation:

[0234] In the drone positioning system, the relationship between RSSI value and distance is expressed using the path loss model:

[0235] RSSI=-(10c log 10 (d)+A);

[0236] Where RSSI is the received signal strength in dBm; d is the distance between the device and the Bluetooth beacon; A is the signal strength value measured at a reference distance of 1 meter; c is the path loss index;

[0237] RSSI = -(10n log 10 (d)+A) is transformed to derive the estimated distance d between the device and the Bluetooth beacon:

[0238]

[0239] Step S5-2: Bluetooth beacon multilateration positioning:

[0240] Assume the coordinates of Bluetooth beacon i are (x i ,y i ), the position coordinates of the drone to be located are (x, y), and the distance between it and beacon i is d i , then the distance relationship between the drone and the beacon is:

[0241]

[0242] Using the distance d between multiple beacons in space and the drone i Construct multiple nonlinear equations and simplify them into linear equations through elimination method, that is, solve the position coordinates of the UAV.

[0243] Step S6: applying Bluetooth linear positioning constraints;

[0244] In traditional Bluetooth-based drone positioning systems, environmental changes may cause Bluetooth RSSI positioning accuracy to decrease or even fail; a model that does not rely on multiple calibration points is required. When positioning in a specific geometric space such as a corridor or a cabinet aisle, applying linear constraints to the drone position can improve positioning accuracy. At the same time, during fingerprint recognition, fingerprint data that does not meet the constraints can be removed to optimize the performance of the drone positioning system. The specific steps of the constraints proposed in the present invention application are as follows:

[0245] The likelihood function derived from the RSSI distance model is used to calculate the distance error from the device to the beacon;

[0246] Create a circular density function around each beacon The radius is determined based on the measured data and all measurements are accumulated and summed. The expression is as follows:

[0247]

[0248] in, To locate the two-dimensional position of the terminal, is the 2D position of the ith beacon, d i is the distance measured by the i-th beacon; the distance d between the drone terminal and the beacon is estimated by measuring the signal strength i , and find a target position x so that the actual distance from the drone to the beacon The distance d from the inferred i The difference between them is the smallest; by minimizing this likelihood function The actual position of the drone can be estimated;

[0249] To further optimize the localization, the position estimate is constrained using the following linear constraints:

[0250]

[0251] Among them, A is the linear constraint matrix, which represents the range of the area to be located;

[0252] Assuming that the area to be located is a rectangle, A and vector b in Ax≤b are expressed as:

[0253]

[0254] Among them, x min and x max Indicates the left and right boundaries of the room, y min and max Indicates the upper and lower boundaries of the room.

[0255] Step S7: multi-source data fusion positioning;

[0256] Acoustic signal positioning has a wide coverage range and high positioning accuracy, but it is easily limited by multipath effects in the complex indoor environment of the substation, so it is suitable for deployment in open indoor scenes. In contrast, Bluetooth devices are low-cost and have a shorter effective working distance, making them more suitable for narrow spaces. By adopting a multi-sensor fusion positioning method, combining acoustic signals and Bluetooth information, a fusion positioning algorithm based on extended Kalman filtering is achieved to achieve full coverage of indoor drone positioning in the substation. The specific steps are as follows:

[0257] Step S7-1: State vector definition

[0258] Set the state vector X of the drone terminal center of mass to include two parts: position p and velocity v:

[0259]

[0260] Step S7-2: Setting the system state transfer equation. The nonlinear state transfer equation f of the system is expressed as:

[0261] X k|k-1 =f(X k-1|k-1 ,u k ,q k );

[0262] Among them, u k is the control input vector, q k is the process noise vector;

[0263] The state transfer of the system follows the kinematic model. The position of the UAV terminal changes with the speed. The state transfer equation describes the change of position and speed, which can be expressed as:

[0264]

[0265] v k =v k-1 +a k-1 Δt;

[0266] Where a is the acceleration and Δt is the time step. Substitute the motion model into the state transfer function:

[0267]

[0268] The corresponding state transfer function f corresponds to the Jacobian matrix F k for:

[0269]

[0270] in, is the partial derivative of the state transfer function f with respect to the state vector X, indicating the degree of local linearization of f with respect to the state; I is the unit matrix, indicating the autocorrelation of the variable;

[0271] Step S7-3: Update the prediction covariance matrix and set the state covariance matrix prediction equation to:

[0272]

[0273] Among them, P is the covariance matrix of the state vector, Q is the covariance matrix of the process noise, and L k is the Jacobian matrix of the noise;

[0274] Step S7-4: Observation model and update equation:

[0275] Considering the fusion of acoustic signals and Bluetooth fingerprint positioning information, the observation equation Z is set k It is the combined information of two sensors:

[0276] Z k =h(X k|k-1 ,r k );

[0277] Among them, h(X) is the observation function, Bluetooth fingerprint and acoustic signal provide different location information, r k is the observation noise;

[0278] The Jacobian matrix H of the observation equation k It is expressed as:

[0279]

[0280] Calculate the residual and convert the sensor's observation value Z k and the state X predicted by the system k|k-1 Compare and calculate the observed residual Y k :

[0281] Y k =Z k -h(X k|k-1 ,r k );

[0282] Among them, Z k represents the observation data of the sensor, h(X) is the observation function, which represents the observation value estimated by the system from the state vector;

[0283] Calculate the innovation covariance matrix and combine it with the observation noise R k and the state covariance matrix P k|k-1 To calculate the innovation covariance matrix S k :

[0284]

[0285] Calculate the Kalman gain and use the Kalman gain Kk Adjust the system's confidence in observed data:

[0286]

[0287] Combined with the predicted state X k|k-1 and the observed residual Y k , using the Kalman gain K k Update the state vector X of the smart terminal k|k :

[0288] X k|k =X k|k-1 +K k Y k ;

[0289] As observation information is continuously introduced, the value of the covariance matrix will gradually decrease:

[0290] P k|k =(IK k H k ) k|k-1 ;

[0291] Among them, H k Jacobian matrix of the observation equation.

[0292] Using the above fusion positioning algorithm, the acoustic signal and Bluetooth positioning data can be fused to achieve accurate position estimation of the drone. By fusing two different positioning data sources, Bluetooth and acoustic signals, the errors caused by the limitations of a single sensor (such as the small Bluetooth coverage range and the susceptibility of acoustic signals to multipath interference) can be reduced, thereby achieving more accurate positioning of the drone in all scenarios. With each iteration, the system updates the position of the drone based on the new observation data, gradually reducing noise and uncertainty, thereby tracking the drone in real time and accurately in complex indoor environments.

[0293] Step S8: Build and update the Bluetooth fingerprint database;

[0294] The RSSI signal strength value received by the drone through the Bluetooth module and the drone position coordinates fused by Bluetooth and acoustic signals are stored in pairs (RSSI, (x, y, z)) to build a Bluetooth fingerprint database. By collecting RSSI data systematically at multiple different locations in the substation room, the accuracy of drone positioning and the robustness of the system can be significantly improved. Figure 3 As shown in the figure, it is a contour map of RSSI signal strength values ​​in a substation room.

[0295] The above description is only an embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A Bluetooth and acoustic signal fusion positioning method for indoor UAV inspection of substations, characterized by: The method comprises the following steps: Step S1: deploying a drone positioning system based on acoustic signals and Bluetooth; Step S2: receiving an acoustic signal, and performing acoustic signal quality evaluation on the received acoustic signal and a reference signal; Step S3: Calculate the arrival time difference of all sound signals and perform position estimation; Step S4: evaluating the acoustic signal localization quality; Step S5: Bluetooth signal reception and multi-lateration positioning; Step S6: applying Bluetooth linear positioning constraints; Step S7: multi-source data fusion positioning; Step S8: Build and update the Bluetooth fingerprint database.

2. According to claim 1, a Bluetooth and acoustic signal fusion positioning method for indoor UAV inspection of substations is characterized by: The step S1 deploys a drone positioning system based on acoustic signals and Bluetooth, specifically: The drone positioning system includes a drone terminal, a Bluetooth beacon, an acoustic signal base station and a central positioning server, wherein N acoustic signal base stations are deployed at the four corners of the ceiling edge to emit predefined acoustic signals with synchronous delay; The Bluetooth beacon is placed on the ceiling above the area to be located so that the Bluetooth messages it sends cover the entire target location area; The drone terminal is equipped with a microphone sensor and a Bluetooth module, the onboard microphone receives a frequency-modulated sound signal from an acoustic signal base station, and the onboard Bluetooth module receives a BLE message with RSSI transmitted by a Bluetooth beacon; The Bluetooth beacon, acoustic base station, drone terminal, and central positioning server that performs positioning communicate with each other.

3. According to claim 1, a Bluetooth and acoustic signal fusion positioning method for indoor UAV inspection of substations is characterized by: In step S2, an acoustic signal is received, and the received acoustic signal and a reference signal are compared to perform acoustic signal quality evaluation, specifically: Step S2-1: Signal strength evaluation: The signal strength evaluation S is calculated based on the power of the received signal and is expressed by the following formula: Among them, P r is the received power of the acoustic signal, P ref is the received power of the reference signal; Step S2-2: Signal-to-noise ratio evaluation: The signal-to-noise ratio is used to evaluate the relative strength of the received acoustic signal and noise and is calculated using the following formula: Among them, P noi is the power of background noise; Step S2-3: Cross-correlation coefficient evaluation: The cross-correlation coefficient evaluates the similarity between the received signal and the reference signal. The cross-correlation function R between the two signals xy (τ) is expressed as: Where x(t) is the reference signal, y(t) is the received signal, and τ is the signal delay; Step S2-4: construct a unified acoustic signal quality evaluation standard.

4. According to claim 3, a method for integrating Bluetooth and acoustic signals for indoor UAV inspection of substations is characterized by: In step S2-4, a unified acoustic signal quality evaluation standard is constructed, specifically: Step S2-4-1: setting the three signal quality evaluation indicators of signal strength, signal-to-noise ratio and correlation between signals as the criterion layer, and setting the comprehensive signal quality evaluation as the target layer; The relative importance of these dimensions is evaluated through expert scoring, and converted into a scoring scale of 1-9 to construct a judgment matrix, where each element in the matrix represents the importance of the i-th dimension relative to the j-th dimension, as follows: Among them, p S,SNR Indicates the importance of signal strength relative to the signal-to-noise ratio, p S,R Indicates the importance of signal strength relative to the cross-correlation coefficient; p SNR,R is the importance of the signal-to-noise ratio relative to the cross-correlation coefficient; Normalize the judgment matrix to obtain the weight of each signal evaluation dimension, that is, the signal strength weight w S , signal-to-noise ratio weight w SNR , the correlation weight w between signals R ; Step S2-4-2: Perform consistency check on the matrix, that is, calculate the consistency ratio CR: Calculate the maximum eigenvalue λ of the judgment matrix max , and calculate its corresponding consistency index CI: Where z is the dimension of the matrix; Introducing the random consistency index RI, for a judgment matrix with a dimension of 3, RI is 0.

58. At this time, If CR is less than a certain value, the consistency of the judgment matrix is ​​acceptable; Step S2-4-3: Construct a comprehensive signal quality evaluation formula using the weights obtained from steps S2-4-1 to S2-4-2: U=w S ·S norm +w SNR ·SNR norm +w ρ ·ρ norm +w CIR ·CIR norm ; According to the comprehensive score U, set the threshold U th Determine the quality of the received signal; if the comprehensive score U exceeds the threshold U th , the signal quality is considered to meet the requirements and is suitable for position estimation; otherwise, the received signal is considered to not meet the requirements.

5. The method for fusion positioning of Bluetooth and acoustic signals for indoor UAV inspection of substations according to claim 1 is characterized in that: In step S3, the arrival time difference of all collected sound signals is calculated and the position is estimated, specifically: Step S3-1: Calculation of the full set of TDOA measurements: The UAV positioning system based on acoustic signals includes M acoustic signal base stations with known positions, which are s i , i=1,2,…,M; For acoustic signal base station i and acoustic signal base station j, the TDOA measurement value r between them ij Defined as: r ij =∥us i ∥-∥us j ∥+n ij +o ij +n ij ; Where u is the unknown position vector of the signal source; s i and j are the position vectors of acoustic signal base station i and acoustic signal base station j respectively; n ij is the line-of-sight measurement noise, which obeys zero-mean Gaussian distribution; o ij is the abnormal data error, caused by measurement noise or equipment error; η ij is the deviation of the non-line-of-sight path, which is a positive value; The TDOA measurements of all acoustic signal base station pairs are constructed into a matrix R: Among them, r ij represents the TDOA value from sensor i to sensor j; Step S3-2: Identify NLOS measurements using cyclic redundancy detection based on geometric constraints: Using the geometric constraint characteristics of the full set TDOA matrix, the outliers are judged by the cyclic sum of the closed path. For the closed path of any acoustic signal base station, its TDOA measurement value satisfies the following cyclic sum condition: r 12 +r 23 +r 31 ≈0； If the cycle sum of a path does not satisfy this condition, that is, |r 12 +r 23 +r 31 |>∈, indicating that there is a non-line-of-sight signal on this path; Ideally, the full set TDOA matrix R should satisfy the rank constraint. When there is only LOS signal, the rank of the matrix should be 2. If the rank of R is greater than 2, it means that the matrix contains abnormal data or NLOS path. Step S3-3: NLOS detection and removal algorithm: Based on the cyclic sum and rank constraints, a set of iterative steps are defined to identify and remove NLOS measurements, as follows: Define the initialization loss function L to evaluate the consistency of the measured data: L=∑ (i,j)∈S (r ij -(∥Γ-s i ∥-∥Γ-s j ∥)) 2 ; Among them, S is the set of base station pairs for TDOA measurement, Γ is the estimated value of the source position, and the optimal estimate is obtained by minimizing the loss function; in the process of minimizing the loss function, the TDOA measurement values ​​of each pair of base station pairs are traversed one by one to check whether the loop and condition are met: |r ij +r jk +r ki |>∈； For paths that do not meet this condition, the TDOA measurement values ​​with large errors are marked as NLOS values ​​and removed from the set S. After removing the NLOS measurement values, the set S is updated, the source position estimate u is recalculated, and the process is iterated until all closed paths meet the loop and condition or the loss function is no longer significantly reduced. Step S3-4: Final position estimation: After removing the NLOS measurements, the final source position is estimated by minimizing the updated loss function: Among them, S LOS is the line-of-sight (LOS) measurement set after NLOS removal.

6. The method for fusion positioning of Bluetooth and acoustic signals for indoor UAV inspection of substations according to claim 1 is characterized by: The acoustic signal localization quality is evaluated in step S4, specifically: Step S4-1: Calculate the mean of the current positioning samples: The sample mean represents the central tendency of multiple measurement results and indicates the optimal estimate of the coordinate position. The expression is as follows: in, and The current position is the mean value of the position estimation under n measurements, x i and i are the x and y coordinates of the ith measurement respectively; Step S4-2: Calculate the standard deviation of the current positioning sample: The sample standard deviation of the positioning coordinates measures the degree of dispersion of multiple measurement results and reflects the distribution of positioning data around the mean. The larger the standard deviation, the more dispersed the measurement results and the greater the uncertainty of positioning. The calculation process is as follows: Among them, s x and y They are the standard deviations of the x and y coordinates of n measurements updated in real time; Step S4-3: Calculate the confidence interval of the current positioning point: Based on the sample mean, standard deviation, and t value, calculate the confidence interval at the current coordinates: Among them, CI x and CI y are the confidence intervals for the x and y coordinates, respectively; the t value is 2.262; The width of the confidence interval of the position estimate indicates the degree of fluctuation of the two-dimensional positioning estimate at that position, as shown in the following formula: Among them, d wx and d wy Represents the width of the confidence interval at the x and y coordinates at the current confidence level.

7. The method for fusion positioning of Bluetooth and acoustic signals for indoor UAV inspection of substations according to claim 1 is characterized by: The Bluetooth signal reception and multi-lateral positioning in step S5 are specifically as follows: Step S5-1: Bluetooth RSSI signal reception and distance estimation: In the drone positioning system, the relationship between RSSI value and distance is expressed using the path loss model: RSSI=-(10clog 10 (d)+A); Where RSSI is the received signal strength in dBm; d is the distance between the device and the Bluetooth beacon; A is the signal strength value measured at a reference distance of 1 meter; c is the path loss index; RSSI = -(10nlog 10 (d)+A) is transformed to derive the estimated distance d between the device and the Bluetooth beacon: Step S5-2: Bluetooth beacon multilateration positioning: Assume the coordinates of Bluetooth beacon i are (x i ,y i ), the position coordinates of the drone to be located are (x, y), and the distance between it and beacon i is d i , then the distance relationship between the drone and the beacon is: Using the distance d between multiple beacons in space and the drone i Construct multiple nonlinear equations and simplify them into linear equations through elimination method, that is, solve the position coordinates of the UAV.

8. The method for fusion positioning of Bluetooth and acoustic signals for indoor UAV inspection of substation according to claim 1 is characterized by: In step S6, Bluetooth linear positioning constraints are applied, specifically: The likelihood function derived from the RSSI distance model is used to calculate the distance error from the device to the beacon; Create a circular density function around each beacon The radius is determined based on the measured data and all measurements are accumulated and summed. The expression is as follows: in, To locate the two-dimensional position of the terminal, is the 2D position of the ith beacon, d i is the distance measured by the i-th beacon; the distance d between the drone terminal and the beacon is estimated by measuring the signal strength i , and find a target position x so that the actual distance from the drone to the beacon The distance d from the inferred i The difference between them is the smallest; by minimizing this likelihood function The actual position of the drone can be estimated; To further optimize the localization, the position estimate is constrained using the following linear constraints: Among them, A is the linear constraint matrix, which represents the range of the area to be located; Assuming that the area to be located is a rectangle, A and vector b in Ax≤b are expressed as: Among them, x min and x max Indicates the left and right boundaries of the room, y min and max Indicates the upper and lower boundaries of the room.

9. The method for fusion positioning of Bluetooth and acoustic signals for indoor UAV inspection of substation according to claim 1 is characterized in that: The multi-source data fusion positioning in step S7 is specifically as follows: Step S7-1: State vector definition Set the state vector X of the center of mass of the drone terminal to include two parts: position p and velocity v: Step S7-2: Setting the system state transfer equation. The nonlinear state transfer equation f of the system is expressed as: X k|k-1 =f(X k-1|k-1 ,u k ,q k ); Among them, u k is the control input vector, q k is the process noise vector; The state transfer of the system follows the kinematic model. The position of the UAV terminal changes with the speed. The state transfer equation describes the change of position and speed, which can be expressed as: v k =v k-1 +a k-1 Δt; Where a is the acceleration and Δt is the time step. Substitute the motion model into the state transfer function: The corresponding state transfer function f corresponds to the Jacobian matrix F k for: in, is the partial derivative of the state transfer function f with respect to the state vector X, indicating the degree of local linearization of f with respect to the state; I is the unit matrix, indicating the autocorrelation of the variable; Step S7-3: Update the prediction covariance matrix and set the state covariance matrix prediction equation to: Among them, P is the covariance matrix of the state vector, Q is the covariance matrix of the process noise, and L k is the Jacobian matrix of the noise; Step S7-4: Observation model and update equation: Considering the fusion of acoustic signals and Bluetooth fingerprint positioning information, the observation equation Z is set k It is the combined information of two sensors: Z k =h(X k|k-1 ,r k ); Among them, h(X) is the observation function, Bluetooth fingerprint and acoustic signal provide different location information, r k is the observation noise; The Jacobian matrix H of the observation equation k It is expressed as:

10. The method for fusion positioning of Bluetooth and acoustic signals for indoor UAV inspection of substations according to claim 9 is characterized in that: The observation update step includes calculating the residual, the innovation covariance, the Kalman gain and the update of the state vector, which is as follows: Calculate the residual and convert the sensor's observation value Z k and the state X predicted by the system k|k-1 Compare and calculate the observed residual Y k : Y k =Z k -h(X k|k-1 ,r k ); Among them, Z k represents the observation data of the sensor, h(X) is the observation function, which represents the observation value estimated by the system from the state vector; Calculate the innovation covariance matrix and combine it with the observation noise R k and the state covariance matrix P k|k-1 To calculate the innovation covariance matrix S k : Calculate the Kalman gain and use the Kalman gain K k Adjust the system's confidence in observed data: Combined with the predicted state X k|k-1 and the observed residual Y k , using the Kalman gain K k Update the state vector X of the smart terminal k|k : X k|k =X k|k-1 +K k Y k ; As observation information is continuously introduced, the value of the covariance matrix will gradually decrease: P k|k =(I-K k H k )P k|k-1 ; Among them, H k Jacobian matrix of the observation equation.

Images