Data Fusion Using a Factor Graph for Ship Tracking in Harbour Scenarios

Transcription

1 Data Fusion Using a Factor Graph for Ship Tracing in Harbour Scenarios Francesco Castaldo Francesco A.N. Palmieri Dipartimento di Ingegneria Industriale e dell Informazione Seconda Universitá di Napoli (SUN) via Roma 29, Aversa (CE)-ITALY france.castaldo@gmail.com ; francesco.palmieri@unina2.it Abstract. Data coming from cameras deployed along an harbour coastline are fused to extract the state of unnown vessels framed by the sensors. We embed the ship dynamic model into a Factor Graph that through probability propagation provides a very flexible merge of sensory data and inferences. Preliminary results and experiments from videos gathered in the Gulf of Naples are reported with a discussion on future trends. Keywords: Data Fusion, Harbour Management, Factor Graph, Ship Model, Camera 1 Introduction State estimation of moving objects in a complex scene is a research topic much explored in recent years and in constant evolution as such technology can provide improvements to safety and management in various application areas (Harbours, Airports, Public areas, etc.). Intelligent and automatic fusion of data coming from different sensor modalities [1], that provide information that differ in type, quality and reliability, requires a framewor that is capable of handling in real time previous nowledge about the model together with environmental constraints and measurements uncertainty. In this paper we consider an harbour scenario, and the state estimation problem for tracing one or more vessels appearing in the field of view of cameras deployed along the coastline [2]. Tracing of multiple targets can be achieved using several and well-nown techniques [3] [4] [5], such as Kalman Filters, Particle Filters, Bayesian Networs, Factor Graphs, etc. We have chosen Factor Graphs, and in particular their normal realizations, also called Forney-Style Factor Graphs (FFG) [6]. Graphical models such as FFGs are very attractive for two main reasons: simplicity and This wor has been partially sponsored by Ministero Infrastrutture e Trasporti, PON , Harbour Traffic Optimization System (HABITAT) with Consorzio Nazionale Interuniversitario per le Telecomunicazioni (CNIT)- Italy.

2 2 F. Castaldo and F.A.N. Palmieri rigorousness. The first feature allows to map into a nice and comprehensible structure very complex problems and algorithms, while the second one guarantees robustness and optimality of the resulting estimates. Factor graphs are also modular, a feature very important in mutable scenarios as harbours are. The paper is organized as follow. In Section 2 we present the ship model used in the algorithm. In Section 3 the factor graph is introduced, with particular attention to the type of messages exchanged between the nodes. Section 4 discusses preliminary results obtained in a test in the Gulf of Naples and Section 5 draws conclusions with suggestions for future developments. 2 Ship Model The discrete-time mathematical model we use for the state of ships sailing near the harbour is based on a single point (usually the ship s barycenter) that moves on the 2D sea surface. We use a modified version of the model presented in [7] [8], considering vessels sailing at almost constant speed and with moderate maneuvering capabilities. Defining x = (x, y, v x, v y ) T the ship state at time, with (x, y ) the GPS ship coordinates (longitude and latitude) and (v x, v y ) ship velocity components, we have the following discrete-time model x = x 1 + T v x 1 + w x, y = y 1 + T v y 1 + w y, v x = v x 1 + w vx, v y = v y 1 + w vy, where T is the sampling time and w = (w x, w y, w vx, w vy ) T is a whitegaussian noise that allows slight changes in ship s position, speed and course. We can write the model in matrix form as x y v x v y = 1 0 T T x 1 y 1 v x 1 v y 1 + w x w y w vx w vy or equivalently as x = Ax 1 + w, (1) 1 0 T 0 with A = T Initial conditions are in the vector x 0 = (x 0, y 0, v x0, v y0 ) T Data Fusion using Factor Graph Sensor data and ship model are embedded here into a Forney-Style Factor Graph [6] [9] [10]. Factor graphs are an emerging framewor that allows to map many,

3 Data Fusion using Factor Graph 3 problems of different ind (signal processing, statistics, data fusion, etc) into a graph. With both simplicity and rigor the model can be manipulated with the information flow that travels bidirectionally for prediction and inference. A factor graph is composed of edges (through which information is propagated) and nodes (through which information is processed). Forward and bacward messages are denoted as f x and b x, where x is the edge variable. Either forward or bacward messages coming out of a node depend on messages entering the node. For the reader not familiar with this framewor, we would lie to emphasize that, even though the propagating equations may appear somewhat unjustified, they are rigorous translation of marginalization and Bayes rule [6]. In this paper we assume that messages are gaussian pdfs, i.e. fully describable by a mean vector m and a covariance matrix Σ. Clearly the state mean at time represents the state expected value, while the covariance represents how that value is dispersed around the mean. Our confidence may evolve in time as the result of information gathered and fused in various parts of the graph. Forward and bacward messages for a variable x are defined as the sets f x = {m fx, Σ fx }, b x = {m bx, Σ bx }. Each graph edge corresponds to a variable and forward/bacward messages can be combined along the same edge to get the variable s best nowledge pdf by the product rule [6] [9]. Given the hypothesis of gaussianity, after the application of the product rule we get again a mean and a covariance matrix that fully describe the result. More specifically, at an edge x from nowledge of forward f x and bacward b x messages, we can compute mean and covariance of x as m x = Σ x (Σ 1 f x m fx b x m bx ), Σ x = (Σ 1 f x b x ) 1. More details about the sum-product rule can be found in [9]. In our tracing application, the factor graph used in this paper, for each discrete time, has the structure shown in Figure 1. The linear A bloc represents the first part of the model defined in (1). The initial forward message is f x0 = {x 0, Σ 0 }, with x 0 initial condition and Σ 0 a fixed covariance. The smaller the covariance s values are chosen, the greater our confidence on initial values are. The following sum bloc adds the white gaussian noise w through the forward message f w = {0, Σ fw } that represents our model s uncertainty. Low values of covariance s norm imply great confidence on model s evolution, while high values imply strongly reliance on sensory data. The equal bloc implements the actual fusion between ship predictions and sensory measurements. The information that converges into this node can be sharp, partial or null: the rules of message propagation and combination guarantee that we always utilize all the information at best. Bloc g combines cameras 2D information (called obs ) to provide on-line measurements of the ship state at any time, as shown in the simplified diagram

4 4 F. Castaldo and F.A.N. Palmieri + = Fig. 1. The -th stage in the factor graph of Figure 2. The camera system, not detailed here, but discussed in [2], consists of a set of N cameras that have been deployed and calibrated using AIS data. Calibration is equivalent to obtaining a prospective homography matrix for each camera. The vision system contains many details, some of which have been reported in [2], that will be discussed elsewhere for space reasons. Essentially, image points are mapped into geographical positions via a 2D reconstruction procedure; after pattern recognition and pixel association the estimates are fused to provide an instantaneous estimate of the ship s parameters. To the purpose of this paper we assume that the camera system at each time provides a message f x 3 on the ships state. Note that the camera system can improve its tracing capabilities using the available b x 3 coming from the factor graph (not discussed here). The bloc g can wor even with one single camera, but obviously more cameras can cover a larger area, providing more reliable and general estimates. In the message f x 3 = {m fx 3, Σ fx 3 }, the covariance matrix Σ fx 3 carries information on how reliable the data is at that moment. Note this model allows time-dependent fusion in which sensory data may carry a very large covariance, which means that at that time camera information may be very unreliable (lost target), or essentially unavailable (poor guess). The nice feature of the factor graphs is that messages can be propagated bidirectionally providing predictions into the future and estimation improvements (smoothing) into the past. To infer into the past, we also need a bacward message coming from the end of the graph. We can model it as b x 2 = {0, Σ bx 2 }, with a very high-valued covariance matrix. Message fusion can be accomplished Σ bx 2 along each edge of the graph, even though the most relevant information is at the end of each graph s element, that is x 2 i = {m x 2, Σ i x 2 }, with i = 1,...,. i

5 Data Fusion using Factor Graph 5... Cam1 Cam2 CamN Fig. 2. A simplified diagram of the multiple-camera system (g bloc) Given inputs f x 2 1, b x 2, f x 3 be computed as and f w, forward and bacward messages can f x 0 = {Am fx 2 1, AΣ fx 2 1 A T }, f x 1 = {m fx 0 + m fw, Σ fx 0 + Σ fw }, f x 2 = {(Σ 1 x 1 x 3 ) 1 (Σ 1 m x 1 m x 3 ), (Σ 1 x 1 = {(Σ 1 f x 3 b ) 1 (Σ 1 x 2 f m x 3 fx 3 b x 0 = {m bx 1 m fw, Σ bx 1 + Σ fw }, b x 1 b x 2 1 x 3 x 1 b x 2 m bx 2 ), (Σ 1 f x 3 = {(A T Σ 1 b x 0 A) 1 (A T Σ 1 b x 0 m bx 0 ), (A T Σ 1 b x 0 A) 1 }. ) 1 }, x 3 b x 2 ) 1 }, 4 Preliminary results Our tracing framewor has been tested on real data coming from two cameras deployed along the Gulf of Naples coastline and calibrated with AIS data [11] of nown ships [2]. The test regards state reconstruction of a large ship appearing in the cameras fields of view. The ship under consideration is one of the vessels used for calibration, therefore its position, speed and course are already nown from AIS. This information is compared with the state provided by the algorithm, in order to assess estimation accuracy. Assuming nown initial state x 0 and, for each discrete step, measures from the cameras f x 3, the algorithm fuses altogether forward and bacward messages, getting ship state estimate, = 1,..., 7. The covariances are diagonal matrices with standard deviations chosen to be as follows: for the initial conditions σ 0 = 10 6 ; for the prediction model σ w = 10 3 ; for the

6 6 F. Castaldo and F.A.N. Palmieri bacward information at the end of the chain σ bx 2 = 10 4 ; for the information 7 coming from the camera system σ fx 3 = 10. In Figure 3 we plot on a Google-Maps image of the Gulf of Naples both the ship GPS positions (red triangles) coming from AIS and the reconstructed positions obtained from our procedure (green asteriss). Data from both cameras and from AIS have been interpolated as they are all available asynchronously. As we can see from the figure, discrepancies are very limited. Figure 4 is a more detailed version of Figure 3, where green crosses represent state estimates calculated at each step. In the algorithm, at each time step a bacward message b x 2 is injected into the graph, and previous states are recomputed, in order to improve the estimation accuracy. Green circles represent approximately covariances calculated at the last step, and indicate how confident we are on the value they represent. They are enlarged times in order to have a comprehensible figure. As stated before, bacward messages enhance previous states reconstruction, therefore going bacwards estimation uncertainty and covariances values decrease. The values go from σ x 2 7 = 10 5 (standard deviation at step = 7) to σ x 2 1 = 10 8 (standard deviation at step = 1). Fig. 3. A Google Maps from the Gulf of Naples, on which true (red) AIS and reconstructed (green) trajectories are superimposed. 5 Conclusions and future developments The framewor described in this paper is intended to be the milestone around which construct an autonomous and intelligent system that provide state information about vessels in movement near the coast. Information about the

7 Data Fusion using Factor Graph 7 Fig. 4. Zoomed version of Figure 3, with green circles that represent a fold magnified indication of the dispersion around the mean (covariance).

8 8 F. Castaldo and F.A.N. Palmieri behaviours of moving targets into the scene can be used to undertstand whether the situation of the harbour is into a normal or a possibly dangerous state. Camera sensory data is fused with the dynamic model and it will be extended to account for multiple targets. Furthermore positional estimates may be available at selective times from different sensor modalities and be fused into the factor graph. The flexibility of the framewor also permits integration of other sensor modalities (e.g. Radar, Lidar, etc.) without much efforts. The estimates coming from these sensors can be fused into the graph, providing better estimates of the object state. This last feature is crucial in complex and changing scenarios as harbours, where sensors can be substituted and modified anytime. Current effort is also being devoted to provide automatic covariance learning through the factor graph s blocs. More specifically, bloc g extracts 2D GPS points using a non-linear operation based on singular values decomposition [2] (not discussed here) and the issue of data dispersion is still an open issue. References 1. Hall, D.L.: An introduction to multisensor data fusion. In: Proceedings of the IEEE 85:6 23, Jan, Palmieri, F.A.N., Castaldo, F., Marino, G.: Harbour Surveillance with Cameras Calibrated with AIS Data. In: Proceedings of the 2013 IEEE Aerospace Conference, Big Sy, Montana, March 2-9, Liggins, M.E., Hall, D.L., Llinas, J.: Handboo of Multisensor Data Fusion: Theory and Practice, Second Edition. September Arulampalam, S.M., Masell, S., Gordon, N., Clapp, T.: A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracing. In: IEEE Transactions on Signal Processing 50: , Feb, Welch, G., Bishop, G.: An Introduction to the Kalman Filter. In: Proceedings of SIGGRAPH. USA: Course, Forney, G.D.Jr.: Codes on graphs: Normal realizations. In: IEEE Trans. Inform. Theory, vol. 47, no. 2, pp , Semerdijev, E., Mihaylova, L.: Adaptive interacting multiple model algorithm for manouvering ship tracing. In: Proceedings of the 1998 International Conference on Information Fusion, Las Vegas, NV, Rong Li, X., Jilov, V.P. : Survey of maneuvering target tracing. part I: dynamic models. In: IEEE Transactions on Aerospace and Electronic Systems 39: , Kschischang, F.R., Frey, B.J., Loeliger, H.-A.: Factor graphs and the sum-product algorithm. In: IEEE Trans. Inform. Theory, vol. 47, pp Feb Loeliger, H.-A., Dauwels, J., Korl, S., Ping, L., Kschischang, F.R.: The factor graph approach to model-based signal processing. In: Proceedings of the IEEE 95: , Jun, IALA Guideline No On an Overview of AIS. International Association of Marine Aids to Navigation and Lighthouse, June, 2011.