WO1988001409A1

WO1988001409A1 - Distributed kalman filter

Info

Publication number: WO1988001409A1
Application number: PCT/US1987/001946
Authority: WO
Inventors: Hubert Chin
Original assignee: Grumman Aerospace Corporation
Priority date: 1986-08-20
Filing date: 1987-08-10
Publication date: 1988-02-25
Also published as: EP0277231A1; JPH01500714A

Abstract

A method and apparatus for processing signals from a sensor system including a distributed Kalman filter utilizing distributed data processing techniques to determine various system states (e.g. position, velocity, attitude, etc.). System state processor (18) and sensor state processors (24, 26, 28) are in communication with each other and receive and calculate error state data. The system errors are fed back to the sensor device processor and both the system and instrument errors are fed back to a data collection processor to continually make corrections in the measurements to compensate for the error estimation.

Description

DISTRIBUTED KALMAN FILTER

The present invention relates to a method and apparatus for data estimation processing/ and more particularly, to a Distributed Kalman Filter utilizing distributed data processing techniques.

Kalman filtering techniques have been developed primarily for estimating state parameters in dynamic systems. Kalman Filters have been used in many applications such as in control systems where real time measurements are not possible. One of the areas of technology where a Kalman Filter is of great importance is in avionics.

There is an increasing demand being placed on tactical aircraft avionic systems and this demand is impacting on the performance of the navigation sub-systems. Present day aircraft utilize an inertial navigation system such as the Strapdown Attitude Heading Reference System (SAHRS) having a plurality of gyroscopes and accelerometers to sense the various parameters necessary for flight control. Another system presently being implemented is the Global Positioning System (GPS), which utilizes a series of eighteen statellites plus three active spares, each circling the earth twice a day in six orbital planes, which will conduct and transmit navigational signals to any location. Each of the above systems as stand alone systems have their own advantages and disadvantages. It has been determined that a combination of the GPS with an inertial navigation system will provide optimal navigation. In an article entitled Integration of GPS With Inertial Navigation Systems, by Cox, Jr, Navigation: Journal of the Institute of Navigation, Vol. 25, No. 2, 1978, PP. 236-245, the author discloses the use of an integrated GPS-inertial filter configuration. Cox acknowledges that his filter is based on a high-order Kalman algorithm that presents problems in execution at a desired rate. In GPS/AHRS: A Synergistic Mix, by Sturza, et al, NAECON 1984, May 1984, pp. 339-348, there is also disclosed an integrated Kalman filter for combining GPS and SAHRS systems. However, no description of the model for implementing the integrated Kalman filter is disclosed. The integration of sensors described in the above systems utilize standard Kalman filtering techniques. However, in the development of mathematical descriptions of the error behaviors, the size of the Kalman filter states will increase markedly, and would lead to a high order model of the system. It follows, that a large number of uncertain variables that contribute to the state of estimation errors, would require a huge computer processing power and. memory.

Recent system literature concerning the subject of real time Kalman. Filtering in the problem of navigationintegration contains two major approaches to handle large, scale state estimation algorithms. In one approach, considerable effort is made for reducing the order of the Kalman filter. Usually this effort has lead to a sub-optimal Kalman filter by partitioning the system states and filter matrices, and rewriting the filter equations in terms of the resulting set of lower order equations. To insist on reduced states that have a computational significance in the application, is to risk degrading filter performance.

An alternative approach is the decentralized Kalman filter in which all sub-systems and their measurements are interconnected. The fundamental idea is to decompose the large system into sub-systems and then manipulate the smaller sub-systems in such a way that the objectives of the overall system are met. Although the decentralized filter is stable, it is not well suited for state estimation. In addition, there is no mechanism for enforcing the interconnection constraints and there are no workable algorithms for a large scale system.

The present invention is directed to a distributed Kalman filter (DKF) for processing signals from at least one sensor device for a system having at least one measurement instrument to provide specific system and instrument data comprising a sensor state processor for receiving instrument error state data from at least one sensor device processor and calculating sensor instrument error data; a system state processor coupled to said sensor state processor for receiving system error state data from said sensor device processor, for calculating system error data and for feeding said system error data back to said sensor device processor; and means for outputting the desired system data and for feeding back the error data to said at least one sensor device processor.

The present invention provides a method for the distributed data processing of signals from at least one sensor device for a system having at least one measurement instrument to provide specific device data, said distributed data processing being performed in a Kalman filter, said method comprising receiving instrument error state data from at least one sensor device processor and calculating sensor instrument error data in a sensor state processor; receiving system error state data from said sensor device processor and calculating system error data in a system state processor; feeding said system error data back to said sensor device processor; and outputting the desired system data and feeding back the error data to said at least one sensor device processor. The present invention is directed to a distributed Kalman filter (DKF) utilizing distributed data processing techniques. The DKF of the present invention is especially useful in integrated multi-sensor systems, such as the SAHRS-GPS system. The DKF provides numerous benefits in solving the burden on computer time by allowing for greater computational capability resulting in improved accuracy, speed and reliability. The DKF of the present invention is a universal filter that can be used to great benefit in the sensor systems for numerous devices. In addition to navigation the distributed Kalman filter can be used for processing data in radar, image processing, optics, television or any system at all where noise presents a problem in determining real time data measurements. Devices in which the DKF would be employed includes aircraft/ spacecraft, land and water vehicles, television and cameras.

The above are merely examples and the use of the DKF is in no way limited to those recited above.

Typically, sensor systems include one or more sensors that collect data needed for the operation of the device, such as navigating a vechicle, identifying a target or focusing a camera. The necessary data is usually provided in various states. For example, for navigation, the states may consist of position, velocity and attitude. These are called system states. In addition, the operation of the sensor itself consists of several states. In the navigation example, the sensor may be a gyroscope which has states that include alignment, coupling and drift. These are called instrument states. Errors are always present in the sensor system since exact measurements and data collection are subject to noise. The DKF estimates the error for all the states which is then fed back to a data collection processor to continually make corrections in the measurements to compensate for the error.

More particularly, the DKF of the present invention processes signals from at least one sensor device of a system to provide specific system and instrument data. A distributed Kalman filter includes a sensor state processor that receives instrument error state data from at least one sensor device processor and calculates sensor instrument error data. A system state processor is coupled to the sensor state processor and receives system state data from the sensor state processor and calculates system error data. The system state processor feeds the system error data back to the sensor state processor. The DKF includes means for outputting the desired system data and for feeding back the error data to the sensor device processor.

A distributed system is defined as any configuration of two or more processors, each with private memory, in which the computations performed in each processor utilizes the combined resources of the component machines. The amount of communication between the processors depends upon the nature of the multi-sensor system. The operating system within each processor determines a communications request and provides the necessary software linkage and signaling required for effective communications. The software to be processed by the distributed computing system consists of functional modules that collectively comprise the distributed program. In addition to the general embodiment of the DKF described above, three embodiments are disclosed in the

GPS-SAHRS environment. Each of the SAHRS and GPS systems have corresponding instrument and system errors represented by a multiplicity of states to be described in detail in the description of the invention. One system is a SAHRS-aided

GPS navigator wherein the DKF includes a GPS state processor and a system state processor. The GPS processor provides data, for example, to compute range and range rates to the four statellites from the Doppler shift of carrier frequency.

This data is. fed through the GPS state processor and system state processor as described with the general DKF. The SAHRS processor provides acceleration and velocity to aid the GPS processor and system state processor.

The second system is a GPS-aided SAHRS navigator which requires the DKF to estimate only the errors in the SAHRS and feedback these errors to recalibrate only the SAHRS. The GPS position and velocity measurements are both supplied through the SAHRS. The third system is a mixed SAHRS/GPS navigator wherein the DKF includes both a SAHRS state processor and a GPS state processor together with a system state processor that are interfaced using distributed processing techniques. The GPS provides range measurements and statellite data. The SAHRS provides acceleration and velocity transformed to the navigation frame together with attitude data. The GPS navigator uses this information for signal reaquisition. The SAHRS uses the GPS position and velocity updates for instrument alignment and calibration.

Figure 1 is a block diagram of a prior art integrated Kalman filter. Figure 2 is a block diagram of the distributed

Kalman Filter (DKF) of the present invention. Figure 3 is a block diagram showing the DKF in a SAHRS/GPS environment. Figure 4 is a block diagram of a DKF in a mixed SAHRS/GPS system. Figure 5 is a block diagram of a DKF in a SAHRS-aided GPS system. Figure 6 is a block diagram of A DKF in a GPS-aided SAHRS system. Figure 7a is a block diagram of a system model of a prior art standard Kalman filter. Figure 7b is a block diagram of a system model of a DKF for the SAHRS/GPS mixed system.

Referring now to the drawings, Figure 1 is a block diagram showing the prior art Kalman Filter arrangement in a typical multi-sensor system. Sensors 1 and 2 compute the error state signals which are then fed into Kalman Filters 1 and 2 respectively. In general, sensors 1 and 2 compute both system errors and sensor errors. After Kalman Filters 1 and 2 process the system errors they feed them into the Kalman Filter 3 which further processes the system errors. This type of situation appears a likely candidate for a decentralized multirate Kalman filter. The prior art system is redundant by processing the same system errors in both Kalman Filters 1 and 2. Furthermore, the integration of Kalman Filters 1 and 2 by Kalman Filter 3 reduces calculation reliability.

In the present invention, a single distributed Kalman filter (DKF) is utilized to process both the instrument and system errors which increases the amount of error states that can be processed. As shown in figure 2, the DKF 10 includes at least two individual processors, processor 12 for instrument errors and processor 14 for system errors. The DKF 10 shown in figure 2 is coupled to a system having a single sensor device processor 16 that can compute a plurality of state signals received from a multiplicity of sensors. The sensor device processor 16 transmits the sensor data to the DKF 10 where it is processed by state processors 12 and 14. Typically, the sensor data is inputted to the instrumentstate processor 12 to process the instrument errors while the system error is fed to the system state processor 14 through the processor 12. Processor 14 computes the system error which is fed back to processor 12. The system and instrument errors are fed back to the sensor device processor 16 which then makes the necessary adjustments to the incoming state signals.

The advantages of the present arrangement are that the instrument error processor is not burdened with filtering the system state errors but filters only the instrument errors while the system state processor filters the system errors received from the sensors. Therefore, less computing time and memory are needed due to the elimination of the redundancy of the system error processor operation. Furthermore, the size of the hardware necessary to accommodate the system is reduced making it applicable for real time operation.

In another embodiment of the present invention, a distributed Kalman. Filter is utilized to integrate two sensor systems. In figure 3, there is shown a DKF 18 arranged to integrate data from a Strapdown Attitude Heading Reference System (SAHRS) and a Global Positioning System (GPS). The SAHRS system includes aircraft rate and acceleration as inputs. Inertial body rate and acceleration data are sensed by an array of skewed inertial components. A sensor redundancy algorithm is performed to select signals, to isolate failures, and to monitor performances. Sensor compensations such as bias, scale factor, and body bending are aligned and the sensory information is resolved along the orthogonal body axes. The orthogonal rate data are corrected for the effects of earth rate and aircraft angular velocity over the earth's surface to obtain the aircraft angular rates with respect to the local level coordinate frame. These rates are utilized to derive the direction cosines and associated vehicle attitude and heading.

The inertial body axis accelerations are transformed to the local level frame, compensated for the effects of gravity and Coriolis acceleration and integrated to obtain local level velocities. The level velocity is divided by the radius of the earth to obtain the angular transport rates for compensation of the measured inertial angular rates. The primary computation of the SAHRS processor 20 is the determination of the direction cosine matrix that relates the aircraft coordinate system to the local level coordinate system. The resultant data are. not sufficiently accurate, specifically in terms of standoff error. The more stringent accuracy requirements for SAHRS dictate that the actual filter is to be designed using sensory outputs and blending the external reference data to estimate error sources.

The basis for the GPS system is the information transmitted by each satellite. This information includes the satellite ephemeris and the time of transmittion of the signal. Transit time is proportional to range, so except for clock bias offset and atmospheric path distortion, the user has a measure of the range to the sending satellites. These measurements are called pseudo-range because of the clock bias. Four simultaneous pseudo-range measurements suffice to allow the user to solve for four unknowns, namely the three components of his position plus his clock bias. Knowing the effects of errors in initial position and initial time on the estimated Doppler shift of the received satellite signals, the receiver can determine the frequency that must be tracked, which is the "apparent" broadcast carrier frequency, usually with a phase-locked loop. Progressive increases in the tracking error and attendant reductions in the detector gain lead to a complete loss of lock. In order to avoid loss of lock, to improve the Doppler estimate, and to reduce the acquisition time the aiding data should be obtained directly from the SAHRS via the DKF.

As shown in Figure 3, the DKF 18 includes a SAHRS sensor state processor 24, a GPS sensor state processor 26 and a common system state processor 28. The SAHRS state processor 24 calculates the instrument error of the SAHRS system while passing the system error data to the system processor 28. Similarly, the GPS state processor calculates the instrument error of the GPS system and passes the system error to the system processor 28. The system error processor 28 passes the system error data back to the SAHRS and GPS processors 24 and 26 respectively. The DKF feeds the SAHRS and GPS error back to the respective sensor processors 20 and 22. The DKF 18 provides the required data output which includes role, pitch , heading, velocity north, east and vertical, latitude, longitude and altitude.

Figure 4 shows another embodiment of present invention wherein the DKF 18 is used to integrate the data from four processors. In addition to the SAHRS and GPS processors 20 and 22, there are also provided a reference sensor processor 30 and a satellite data processor 32. The reference sensor processor 30 includes a magnetic heading reference sensor for determining pressure, altitude, and true airspeed. To insure a bounded heading error in the presence of the SAHRS sensory errors, an external magnetic heading reference (flux valve) is selected. Flux valves are utilized to provide accurate long term heading. The flux valve data and gyro-driven heading data are combined via the filter to achieve both short-and long-term heading accuracy. The calculation of vertical velocity by the SAHRS algorithm requires an external reference to ensure stable velocity data. The accelerometer bias and imperfect gravitational correction will result in an unbounded vertical velocity in a relatively short time. In order to bound the vertical velocity error, it is necessary to. utilize pressure altitude. The local level velocities are utilized in the calculation of the angular transport rates over the earth's surface. These angular rates are transformed into projections along the vehicle body axes to compensate for the measured angular rates. Without the true airspeed as a reference velocity, the attitude and velocity errors will contain the Schuler oscillations in the presence of certain component errors.

The processor 24 contains 33 states derivated from the SAHRS sensor error model. The gyro error model is given as the following five classes: Scale factor errors, three states; Misalignment coupling errors, six states; Bias errors, three states;

Mass unbalance drift errors, three states; Random noise errors, three states.

The model for the accelerometers can be described as the following classes:

Scale factor errors, three states; Misalignment errors, six states; Bias errors, three states; Random noise errors, three states.

A global network of satellites can be configured so that at least four different satellites are above the local horizon for almost every point on or near the earth. The selection of these four satellites has a great influence on the accuracy of a navigation fix. The satellite data processor 32 selects the proper satellites. The satellite selection algorithm consists of the following four steps:

Step one - Select the first satellite with the largest elevation angle;

Step two - Choose the second satellite near to the first one to 110 degrees;

Step three - Determine the third satellite with optimum geometry for visibility;

Step four - Select the last satellite with the property of the minimum geometric dilution of precision.

The satellite motion algorithm determines the position of satellites by the satellite equations of motion. These equations can be expressed in Euler-Hill form, which is a rotating coordinate system defined by right ascension of ascending node, orbital inclination, and latitude. There exists an orthogonal matrix that transforms the position vector of a satellite in the Euler-Hill rotating form to the Cartesian coordinate of the inertially fixed geocentric system. The purpose of this algorithm is to develop

Lagrange's equations of satellite motion of a perturbing acceleration in the Euler-Hill rotating frame, in terms of the angular velocity vector and eccentricity vector, the nonsingular orbital elements ranges and range rates are determined by the transformation.

The processor 26 contains 10 states derived from the GPS sensor error model. They are three range measurements states, three range rate states, one clock state and one clock rate state. The processor 28 contains 9 states derived from the aircraft attitude, position, and velocity. The Reconfiguration Data Management System 34 includes algorithms to perform failure monitoring, failure isolation, configuration selection, and data normalization. In addition, analytic testing calculations are performed to minimize overall hardware requirements. The normalization computation process is the final output, parameter computation, which uses best-estimate data to derive the output parameters.

The GPS receiver provides pseudo-range and deltarange measurements, and satellite data. The SAHRS provides acceleration and velocity transformed to the navigation frame and attitude data. The GPS navigator uses this information for signal reacquisition following intervals for signal outages (resulting from antenna shadowing, bad geometry; and high dynamic maneuvering). The SAHRS uses the GPS position and velocity updates for alignment and calibration of its instruments. The accurate position fixes from the satellite data can not only prevent long-term inertial error growth, but may allow various inertial errors to be estimated in real time and thus compensated for. The error model of the filter is obtained by augmenting the state vector of the GPS-aided SAHRS error model by 10 elements.

These 10 elements represent the range, range-rate, clock bias and clock rate of GPS correlated errors. The error model of the total states is 46 and the update interval is one second. Figure 5 shows the DKF 18 arranged as a GPS aided SAHRS navigator. One way of combating long-term inertial error growth from the SAHRS is to periodically reset the user position coordinates using an accurate fix from GPS. This configuration requires the DKF to estimate only the errors in the SAHRS and feed back these errors to recalibrate only the SAHRS. The GPS position and velocity measurements are both supplied to the SAHRS. The system then represents the updated states that will be subsequently propagate 50 iterations through time until the period of a one second update cycle. A 36-state filter is implemented in the GPS-aided SAHRS navigation set. These error states consist of the six acceleration errors, nine gyro errors, 12 misalignment errors of both accelerometers and gyros, and nine system errors.

The system of Figure 6 shows the DKF 18 implemented as a SAHRS-aided GPS navigator. The GPS receiver provides the data necessary to compute ranges and range-rates to the four satellites from the Doppler shift of carrier frequency. There are two important errors that occur in making these range and range-rate measurements. The first one is caused by the user's clock not being perfectly synchronized with the sattelite clock System. The second error is caused by an oscillator frequency error relative to the transmitted frequencies of the satellites.

The SAHRS provides acceleration and velocity to aid the receiver in the phase-lock loop. The DKF is formed in a two-stage process. The first stage estimates position from GPS pseudo-range measurements and velocity inputs. The second stage uses range-rate measurements and the output from the first stage, plus acceleration inputs. The filter formalism requires 16 error states; they are four range measurements, four range-rate measurements, three gyro biases, three accelerometer biases, and the GPS receiver clock bias and bias rate. Range measurement residual is computed five times per second. The measure vector is based on the SAHRS computation being available 50 times per second. Algorithm design addresses not only the design of analytic estimation algorithms, but also the design of implemental procedures such as one whose function is to detect and respond to white noise in measurements. The design process includes mapping, these algorithms into a system of software procedures that, when executed on some target equipment, will interact correctly with the environment and among themselves, and will also satisfy the real-time constraints of the problem.

The symbols and subscripts in the following discussion are defined as follows: For the i-th subsystem at the k-th update time, X_i ,_k = state vector, z_i,k = measure vector , v_i,k = white measurement noise vector, w_i ,K = input white noise vector. F_{ij ,k} = the state transition matrix from the j-th subsystem state vector to the i-th subsystem state vector. H_{ij ,k} = the linear connection matrix from the i-th subsystem state vector to the j-th subsystem measure vector. In the development of a distributed Kalman filter, the starting point is derived from the discreet system model of standard Kalman equations; then, the partition is taken to the desired subsystems. The system is described by the following linear vector equation:

^X _k+1 = ^Fk^xk^+wk^. (1)

Here, w_k is the system noise and is a zero-mean white noise process with covariance:

Cov = Q_kδi,,j, E[w_k]=0 (2) in which Q_k is

a nonnegative definite matrix and δi,,j is the

Dirac delta function.

The subscript is a discrete filter update time argument that k,,j >0. System equation is often referred to as the system model, since it describes the basic information that we are trying to determine. The state vector, ] X, :k^θ/, is observed by means

of a noisy mechanism of the zk = H^T _kX_k+V_k, (3) where the measurement noise v_K is a zero-mean white noise process with: Cov E[v_k]=0, (4)

hich R_K is a definite matrix The measurement equations is called the observation model. For simplicity, w and v are assumed uncorrelated so that:

Cov = 0, for all j and k. (5)

The initial value of X is a random variable with: E

[X₀] = X(, and Var ) X, = P_O (6)

Also, it i

sss cassuumed that = 0, for all k.

The global state vector,X_k, can be partitioned into three substate vectors in which X_{1 ,k} is the sensor-one state vector, X_2,k the sensor-two state vector, and X_{3 ,k} the system state vector. This scheme is depicted in Fig. 7 and forms a distributed computing system model. One of the differences between a distributed job and a conventional one is that a job may potentially execute on separate processors to provide coherence to a set of inputs. Then,

(9)

These forms are expanded and rewritten in the following three separate systems.

Sensor-one state space equation

X₁,k+1 = F_11,KX_1,K + F_12,kX_2,k + F_13,kX₃,_k + W₁,k (10)

Z_1,k = H₁₁,_k ^TX_1,K+ H_21,k ^TX_2,k + H_31,k ^TX₃,k + v_1,k (11)

Sensor-two state space equation:

X_2,k+1 = F_22,kX_2,k + F_21,kX_1,k + F_23,kX_3,k + W_2,k (12)

Z_2,k = H_22,k ^TX_2,k + H _12,k ^T _X1,k + H_32K, ^TX_2,k+ V _2,k (13)

System state-space equations:

^X3,K+1 ^{= F}33,k^X3,k ^{+ F}31,k^X1,k ^{+ F}32,k^X2,k ^{+ w}3,k (14)

^Z3,k ^{= H}33,k^TX_3,k ^{+ H}13,k ^TX_1,k ^{+ H}23,k ^TX_2,k ^{+ V}3,k (15)

Since sensor X_1,k and X_2,k states are almost independent, let

F₁₂ = F₂₁ = 0, H₂₁ = H₁₂ = 0. (16) The standard Kalman filter is a linear, descrete-time finite dimensional system. The equations are summarized for convenience as follows: The filter is initialized by:

V (17)

The estimates are: T (18)

(19)

The measurement update equations are:

(20)

' (21)

V | (22)

where, based on a set of sequential observations: r (23) ] (24)

| i (25) A further extension of the standard Kalman filter yields three nonlinear subfilters that are no longer linear and the performance is different from the original one. For some, partition of substate vectors may diverge and be effectively useless, whereas for other selections it may perform well. In order to ensure stability of the distributed Kalman filter for certain coordinate basic selections, one important property is to make sure that the individual processors can accomplish a global effect, executing code and data, and working together to complete an estimation task.

Three subsystem models:

^Xi,k+1 = f_i,k (X_{i ,k}) + w_i ,k (26)

z_i,k = ^hi,k^(Xi,k^{) + v}i.k (27)

where the functions of f_k , h_k are nonlinear, and i = 1,2, and 3.

(28)

where = partial derivative. Approximations are introduced to drive a clearly suboptimal filter for the model.

A A

) | Then the model is as:

(31)

^Xi,k+1 = ^(Fii,k^{) X}i,k ^{+ W}i,k ^{+ U}i,k

(32)

^Zi,k = ^(Hii,k^{) X}i,k ^{+ V}i,k ^{+ Y}i,k'

(33)

(34)

(35)

(36)

(37)

(38)

Extended Kalman filter ecuations are:

Figure 7a represents the continuous system model of a standard Kalman filter shown in Figure 1. The states to be estimated must be modeled in the following vector form:

X = F X + w The measurement relationship connecting the noisy measurement vector Z to the state vector X must be of the form:

Z = H X + v The method of processing in channel 40, includes the input white noise vector, w = [w_1,w₂,w₃]^T , combined in a combiner 42 with previous state vector X = [X₁,X₂,X₃] T which has been multiplied in multiplier 44 by the linear connection matrix F in channel

46, to produce the derivative of the present state vector,

• • • •

X ⁼ [X₁ ,X₂,X₃ ] ^T

The output is passed through an integrator 48 to produce present state vector X. The present state vector may go through channel 46 for re-input to combiner 42 and may stay on channel 40 for input to multiplier 50 to be multiplied by linear connection matrix H. The output of multipler 44 is combined in the combiner 42 for estimating the next state vector. The output of multipler 50 combines white measure noise sequence v = [v₁,v₂,v₃] T in the combiner 52 to produce the present measurement vector Z = [Z_1,Z₂,Z₃]^T Based upon the system model in Fig. 7a, the equations of the standard Kalman filter are presented in equations (17) to (25).

Figure 7b shows the method of distributed processing where the input white nose vector, w₁ is in channel 60, w₂ is in channel 62 and w₃ is in channel 64.

Processor 24 of the Fig. 7b shows the input white noise component w₁, combined in a combiner 66 with previous state vector X₁, which was multiplied in multiplier 68 by the linear connection matrix F₁₁ in channel 70, with previous state vector X₂, which was multiplied in multiplier 72 by the linear connection matrix F₁₂ in channel 24, and with previous state vector X₃, which was multiplied in multiplier 76 by the linear connection matrix F₁₃ in the channel 78. The output from the combiner 66 produces the derivative of the present state vector, X₁. The vector X₁ is passed through an integrator 80 to produce present state vector X₁. The present state vector X₁ will go through channel 70 and be multipled by F₁₁ for re-input to combiner 66, stay on channel 60 and be multiplied by linear connection matrix H₁₁ in a multipler 82, and be sent to processors 26 and 28. The X₂ from processor 28 is multiplied by H₁₃ in the multipler 88 of channel 90. The sum of the outputs from channels 60, 90, and 86 are combined with white measure noise sequence, v₁ in a combiner 92 to produce the present measurement component Z₁. Processor 26 of Fig. 7b shows the input white noise component w₂, combined in a combiner 94 with previous state vector X₂, which was multiplied in multiplier 96 by the linear connection, matrix F₂₂ in channel 98, with previous state vector X₃, which was multiplied in multiplier 100 by the linear connection matrix F₂₃ in channel 102, and with previous state vector X₁, which was multiplied in multiplier 104 by the linear connection matrix F₂₁ in channel 106. The output from the combiner 94 produces the derivative of the present state vector, X₂. The vector X₂ is passed through an integrator 108 to produce present state vector X₂. The present state vector X₂ will go through channel 98 to be multiplied by F₂₂ for re-input to combiner 94, stay on channel 64 and be multiplied by linear connection matrix H₂₂ in the multiplier 110, and is sent to processors 24 and 28. The vector X₁ from processor 24 is multiplied by H₂₁ in the multipler 112 of channel 114 and the vector X₃ from processor 28 is multiplied by H₂₃ in the multiplier 116 of channel 118. The sum of the outputs from channels 64, 118 and 114 are combined with White measure noise sequence, v₂ in a combiner 120 to produce the present measurement component Z₂.

Processor 28 of the Fig. 7b shows the input white noise component W₃ , combined in a combiner 122 with previous state vector X₃, which was multiplied in multiplier 124 by the linear connection matrix F₃₃ in channel 126, with previous state vector X₂, which was multiplied in multiplier 128 by the linear connection matrix F₃₂ in channel 130, and with previous state vector X₁, which was multiplied in multiplier 132 by the linear connection matrix F₃₁, in the channel 134. The output from the combiner 122 produces the derivative of the present state vector, X₃. The vector X₃ is passed through an integrator 136 to produce present state vector, X₃. The present state vector X₃ will go through channel 126 to be multiplied by F₃₃, for re-input to combiner 122, stay on channel 62 to be multiplied by linear connection matrix H₃₃ in the multipler 138, and be sent to processors 24 and 26. The vector X₁ from processor 24 is multiplied by H₃₁ in the multiplier 140 of channel 142 and the vector X₂ from processor 26 is multiplied by H₃₂ in the multipler 144 of channel 146. The sum of the outupts from channels 62, 142, and 146 is combined with white measure noise sequence, v₃ in a combiner 148 to produce the present measurement component Z₃. Based upon the system model in Fig. 7b, the equations of the distributed Kalman filter are implemented in accordance with equations (39) to (43). The dashed lines and nodes are represent optional choices.

The system model of Figure 7b represents the operations of the DKF which is implemented across a number of physical devices that communicate with each other. The algorithm of the DKF operates on the system errors in order that they will be eliminated out of the system providing improved performance as the end result. An advantage of the DKF of the present invention is an approximate 78% reduction in the total number of operations and 57% decrease in required computer memory. In the mixed SAHRS/GPS system, this results in the optimal combining of the excellent long term performance of GPS with the good short term performance of SAHRS. While illustrative embodiments of the subject invention have been described and illustrated, it is obvious that various changes and modifications can be made therein without departing from the spirit of the present invention which should be limited only by the scope to the appended claims.

Claims

WHAT IS CLAIMED IS:

1. A distributed Kalman filter for processing signals from at least one sensor device for a system having at least one measurement instrument to provide specific system and instrument data comprising: a sensor state processor (12) for receiving instrument error state data from at least one sensor device processor (16) and calculating sensor instrument error data; a system state processor (14) coupled to said sensor state processor (12) for receiving system error state data from said sensor device processor (16), for calculating system error data and for feeding said system error data back to said sensor device processor (16); and means for outputting the desired system data and for feeding back the error data to said at least one sensor device processor (16).

2. The distributed Kalman filter of Claim 1 wherein said system is a navigation system, such as a Strapdown Attitude Heading Reference System (SAHRS) or a Global Positioning System (GPS).

3. The distributed Kalman filter of Claim 2 wherein both of said SAHRS and GPS navigational systems are coupled to said distributed Kalman filter.

4. The distributed Kalman filter of Claims 1, 2 or 3 wherein said distributed Kalman filter network includes a SAHRS sensor state processor (24) and a GPS sensor state processor (26), both of said SAHRS and GPS sensor state processors being coupled to said system state processor (28).

5. The distributed Kalman filter of any one of the Drecedinq claims wherein both of said sensor (24,26) and system (28) state processors include first means (66,94,122) for combining input signals having noise with a first sensor present state vector and a system present state vector to produce a derivative sensor vector and means (80,108,122) for integrating said derivative sensor vector to produce said sensor present state vector, and include means for combining a second sensor present state vector in said first combining means.

6. The distributed Kalman filter of Claim 5 wherein both of said sensor (24,26) and system (28) state processors include first (68,96,128) and second (76,100,132) means for multiplying both said first and second system present state vectors by first and second matrices prior to being combined in said first combining means and including .third means (72, 104,124) for multipying said second present state vector by a third matrix prior to being combined in said first combining means.

7. The distributed Kalman filter of Claims 5 or 6 wherein both said sensor (24,26) and system (28) state processors include second means (92,120,148) for combining at least two of said present state vectors with a noise vector to produce a present measurement signal and wherein said first and second sensor present state vectors and said system present state vector are combined in second combining means.

8. The distributed Kalman filter of Claims 5,

6 or 7 including means for multiplying each of said sensor, and system present state vector by first (82,110,146), second (88,116,138) and third (84,112,144) matrices respectively prior to being combined by said second combining means.

9. A method for the distributed data processing of signals from at least one sensor device for a system having at least one measurement instrument to prrovide specific-device data, said distributed data processing being performed in a Kalman filter, said method comprising: receiving instrument error state data from at least one sensor device processor and calculating sensor instrument error data in a sensor state processor; receiving system error state data from said sensor device processor and calculating system error data. in a system state processor; feeding said system error data back to said sensor device processor; and outputting the desired system data and feeding back the error data to said at least one sensor device processor.

10. The method of Claim 13 wherein said system is a is a navigation system such as a Strapdown Attitude

Heading Reference System (SAHRS) or a Global Positioning

System (GPS).

11. The method of Claim 10 including coupling both a SAHRS and GPS navigational system to said distributed Kalman filter.

12. The method of Claims 9, 10 or 11 wherein receiving and calculating instrument and system error state data includes combining input signals having noise with a first sensor present state vector and a system present state vector in a first combining means to produce a derivative sensor vector and integrating said derivative sensor vector to produce said sensor present state vector and combining a second sensor present state vector in said first combining means.

13. The method of Claim 12 including mutiplying both said first and second system present state vectors by first and second matrices prior to being combined in said first combining means; and multiplying said second present state vector by a third matrix prior to being combined in said first combining means.

14. The method of Claims 12 or 13 including combining at least two of said present state vectors with a noise vector in a second combining means to produce a. present measurement signal, and multipying each of said sensor and system present state vectors by first, second and third matrices respectively prior to being combined by said second combining means.