Rao-Blackwellized Point Mass Filter and Its Application to Tightly-Coupled INS/TRN Integration
Chang-Ky Sung, Seong-Ho Nam, Jung-Shin Lee, Myeong-Jong Yu, Agency for Defense Development, Republic of Korea
Terrain Referenced Navigation(TRN) is a technology estimating vehicle's position by comparing a measured terrain height from radar altimeter with a priori equipped terrain database.
TRN is a well-known complementary solution when GNSS, using weak signals from satellites, is unavailable in hostile jamming or underwater environments.
TRNs are classified into two types according to their data processing strategies: Batch processing type which estimates position at a moment after profiling measurements for few seconds, and sequential processing type which provides estimated position at every time when measurement information is upcoming.
Sequential processing type TRN which is more adequate method to determine good or bad measurements requires nonlinear filtering techniques due to unpredictable and highly nonlinear characteristic of terrain itself.
There are lots of algorithms applicable to sequential processing TRN, EKF(Extended Kalman Filter), Unscented Kalamn Filter(UKF), Bank of Kalman Filter(BKF), Particle Filter(PF), Point Mass Filter(PMF) and so on.
It is possible to construct single filter structure for INS/TRN integration, when EKF or UKF are used for it.
Searching area, however, is limited to tens of meters because linearizations of terrain or approximations of probability density function(pdf) to normal distribution are included.
On the other hand, since PF or PMF are numerical approximation technique for Bayesian filtering, it is possible to control searching area according to the number of particles in PF or resolutions of grid distribution in PMF.
But there is an unavoidable drawback that the required computational power to implement them is increased exponentially according to the increase of dimension of state variable.
INS/TRN integration structure can be divided into three major parts as follows:
(1) Single TRN filter structure without INS aiding filter
(2) Cascaded structure of the INS aiding filter following TRN filter (Loosely-coupled)
(3) Single structure integrating TRN filter and INS aiding filter (Tightly-coupled)
A second or third order TRN algirithm with two horizontal position errors and vertical error as state variables require position displacements during processing interval to access terrain database represented in terms of absolute positions.
In structure (1), estimation performances may be degraded over time because degree of uncertainty in input due to velocity errors of INS might be increased, whereas degree of uncertainty in loosely-coupled structure can be constrained because velocity errors can be compensated by INS aiding filter estimating INS navigational errors including sensor bias errors.
There might be performance degradation in the second stage of INS aiding filter because measurement inputs from TRN filter are no more white gaussian noises.
Tightly-coupled structure can overcome the problem in loosely-coupled structure, but it is issued the realtime implementation of Bayesian filtering with thirteenth or fifteenth order state variables.
However, in the case that system and measurement model can be expressed in the form of state variables seperated into a nonlinear part and a linear part, Rao-Blackwellized PF(RB-PF) or PMF(RB-PMF) is applicable and, depending on the model structure, a significant reduction in computation is possible.
In this paper, for performance enhancement of PMF based TRN which has higher performance and is known to be more robust that PF based it, a more realistic new RB-PMF algorithm and two TRN types using proposed RB-PMF, third order loosely-coupled TRN and fifteenth order tightly-coupled TRN are proposed.
Previous RB-PMF algorithm is not decomposed into tow stages of time propagation and measurement update so it's impossible in real estimation problems where measurements are intermittently unavailable.
To deal this problem, new RB-PMF algorithm which follows same framework with RB-PF structure which is decomposed into five stages, two stages for nonlinear states and three states for linear states, is proposed.
Next, previous third order PMF based TRN algorithm including altitude error is reconstructed with new RB-PMF.
Previous one requires three dimensional convolution operation in time propagation but new one performs only two dimensional convolution because altitude error term is considered as linear state and it can be seperated from the nonlinear operation.
Finally, previous tightly-coupled INS/TRN filter treated just eight states rigorously selected, three position errors and three attitude errors, two gyro biases and Sampling Importance Resampling(SIR) PF algorithm was applid for its implementation.
Here, it was assumed that Doppler Velocity Log(DVL) is used to acquire accurate velocity information in underwater.
However, there are no accurate velocity sensors for aerial vehicle and it is necessary to construct fifteenth order filter to fully perform the INS aiding function.
Therefore, instead of eighth order INS/TRN filter based on SIR-PF, fifteenth order INS/TRN filter based on RB-PMF including whole INS navigational and sensor errors is proposed.
Simulations are performed to show the effectiveness and performance enhancement of proposed algorithm and methodologies and the results are presented.
For performance comparison affected by INS velocity errors, simulation trajectory is selected to include rough terrains and flat terrains.
1 Nm/hr grade of INS and radar altimeter with 2% of scale factor error, barometer with constant bias error of 30m are applied for simulation.
Characteristics of each structure is described in terms of performance in terrain roughness and computational time consumption.