Control Loop Sensor Calibration Using Neural Networks for Robotic Control

Whether sensor model’s inaccuracies are a result of poor initial modeling or from sensor damage or drift, the effects can be just as detrimental. Sensor modeling errors result in poor state estimation. This, in turn, can cause a control system relying upon the sensor’s measurements to become unstable, such as in robotics where the control system is applied to allow autonomous navigation. A technique referred to as a neural extended Kalman filter (NEKF) is developed to provide both state estimation in a control loop and to learn the difference between the true sensor dynamics and the sensor model. The technique requires multiple sensors on the control system so that the properly operating and modeled sensors can be used as truth. The NEKF trains a neural network on-line using the same residuals as the state estimation. The resulting sensor model can then be reincorporated fully into the system to provide the added estimation capability and redundancy.


Introduction
In the target tracking applications, sensor systems are placed in various locations about the region of interest and provide measurements that are combined over time to provide an improved picture of the region.One of the significant problems with target tracking systems is that the sensors might not be properly calibrated, aligned, or modeled.This can wreak havoc with the performance of the involved tracking systems.When calibration and alignment are the main factors, the problem is referred to as sensor registration [1,2].Since the real-time activities of a tracking problem cannot be postponed, the correction of the registration or the mismodeling must be performed online.This problem can extend to other applications, including that of feedbackcontrol systems.
While some control applications use a single sensor to provide measurements that are used in the feedback loop, many control systems use several sensors to provide the necessary measurements.Multiple sensors are used for a variety of reasons.Often times the sensors add redundancy, such as with navigation systems where multiple inertial navigation systems (INSs) are employed or for safety considerations to provide back-up in case of failure [3].Sometimes multiple sensors are employed because more accurate sensors cannot provide measurements at the necessary update rates.In such cases, less accurate sensors might be used to provide reports at the sampling time between the measurement reports of the more accurate system.Finally, additional sensors are added in that they provide measurements that contain directly observable state information.In mobile robotic systems, for example, sensors include position sensors to determine absolute position, such as a global position system (GPS), or relative position such as radars, sonars, and imagery equipment, as well as speedometers and possible accelerometers that provide the velocity states more directly than the indirectly observed values from position.
While additional sensors are often used to provide improved overall accuracy, errors can arise in the accuracy of the measurements provided to the control law by individual sensors.This occurs with tracking sensors and is well known with gyroscopes in an INS.While a sensor that drifts from its calibration point in a nonrandom and identifiable manner can be recalibrated, there is no remedy other than redundancy or replacing a sensor that fails or breaks completely since it is unable to provide a meaningful measurement.For a sensor that can be recalibrated, online calibration is desired because taking the inaccurate sensor offline may not be possible in the short term or could have a deleterious effect on the performance of the controller.When a closed-loop control system relies upon a sensor with an error, the result is that the control signal varies from the value needed to achieve the desired response.These effects are evident, even in a linear system [4].Sensor calibration is thus an important issue for the control design [5].For nonlinear systems, these issues become even more pronounced and can lead to a more significant effect on stability, as the feedback may not fall within the Lyapunov stability criteria [6].
In this work, an online calibration technique is proposed for the case where the control law utilizes a state estimator [7].The approach uses a technique referred to as a neural extended Kalman filter (NEKF) [8][9][10].The NEKF has the benefit of being able to train a function approximation online and provide the state estimation simultaneously.The NEKF provides a unified approach using the same residual for both components.Techniques such as in [11], in contrast, can provide the desired sensor correction for sensors in a control loop but are not integrated to the state estimator and corrections are performed outside of the control loop.The technique applied to this sensor correction problem is based upon an approach that was developed for a multisensor tracking system where the sensor model was in error or a sensor registration was detected [12].In that development, one sensor was considered to be local while the other was considered to be off-board.All corrections were made to the off-board reports relative to the local reference frame.Unlike the tracking problem where the estimator is open loop, control applications require the consideration of closed-loop issues including stability.The NEKF is used to recalibrate the sensor model based on online operational measurements while in the control loop.It also provides the state estimation for the control law.Thus, the proposed technique estimates the states and provides the training paradigm in a single algorithmic implementation [13,14], unlike other neural network sensor modeling techniques [15][16][17].The software correction to the actual sensor measurements can be applied to the case where the sensor is still operational but poorly calibrated.
The NEKF algorithm requires a truth measurement from which to calibrate the sensor.Using additional sensors on the dynamic system, a poorly calibrated sensor can be modeled such that its reporting errors are removed prior to their incorporation to the feedback loop.
In Section 2, the NEKF algorithm and its implementation in the control loop is developed.Section 3 provides the first example system and the performance results of the online calibration using the NEKF for a two-sensor moving platform that has a range-bearing sensor and a miscalibrated velocity sensor.A navigation problem where an intermittent use of a position sensor such as GPS is available is presented in Section 4. These examples show the benefits of the NEKF calibration.

Recalibration Approach
In this effort, a recalibration technique that can be used online is developed.The approach relies upon other sensors in the multisensor system operating properly which are used to provide a level of truth from which to calibrate.The sensor to be recalibrated may have some inaccuracy but cannot be irreparably damaged.
The dynamics of a nonlinear system can be modeled as a set of recursive difference equations: where x k is the state vector representing the system dynamics, f(•) is state-coupling model, r k is the reference input vector, z k is measurement vector, also considered the report from the sensor system, u k is an external input to the sensor system often simply the reference signal, and h(•) is the outputcoupling function [18].The vectors ν and η represent noise on the system states and the measurement states, respectively.If the reference input r is assumed to be both the reference signal to the system and the external input to the sensor, then r is considered to be the same as u.Then, the state estimation model used in the control law would be rewritten as The accuracy of the models in the estimator determines the accuracies of state estimates.For this effort, it is assumed that the state-coupling function is very accurate while at least one of the sensor models has some inaccuracy.The error in this model is defined as To reduce the measurement error, the function ε needs to be identified.A neural network that satisfies the function approximation requirements of the Stone-Weierstrauss Theorem [9] is proposed to approximate the error function ε.Such a neural network can be defined as where w are the weights of the neural network, which are decomposed into the set of input weights ω and the set of output weights β.The hidden function g(•) is a sigmoid function: This creates a new measurement and more accurate measurement model: where e ε .
The extended Kalman filter (EKF) is a standard for nonlinear estimation in a control loop [4,19].An approach developed in [7] referred to as a neural extended Kalman filter provides both the state estimation of the system and the training of the weights [20] of a neural network using the same measurement estimates.
The NEKF is a coupled state-estimation and neuralnetwork training algorithm and has similarities to parameter estimation approaches [21].In such implementations, the state vector of the EKF contains both the system states and the parameters of the model being estimated.Parameter estimation, though, is based on a known model structure.The neural network of (4), in contrast, is a general function with its weights as the parameters.Therefore, the augmented state of this NEKF is the state estimate and the input and output weights of the neural network: Incorporating this new state and the neural network affects all of the Kalman filter equations.In the Kalman gain equation and the state error covariance update equation, ( 8) and (10), the Jacobian of the output-coupling function is properly augmented.In (9), the neural network is incorporated into the state update.Finally, the prediction equations, (11) and (12), must be properly augmented for the augmented state.Thus, the NEKF equations for the sensor modeling become The augmented Jacobian of the sensor function model, is therefore defined as The necessary coupling between the weights and the dynamic states occurs as a result of the output-coupling Jacobian.This coupling permits the weights to be completely observable to the measurements and to train off of the same residual as the system states.This also implies that the NEKF trains the neural network online without the need for additional offline training sets.The NEKF defined in ( 8)-( 12) requires a truth measurement for the neural network to learn.For a target tracking application where a surveyed target can be used to provide ground truth, this implementation is appropriate [9].Control applications are different in that such "truth" is unavailable.Instead, the state of the system dynamics must be generated using sensors in the control system that are more accurately modeled.This requires a modification of the NEKF design to utilize the existing state estimates and the measurements from the other sensors to correct the outputcoupling function of the mismodeled sensor using the neural network.
For this implementation, the detection of a sensor failure is assumed to have occurred.Such detection techniques abound in tracking theory and similar techniques exist in dynamic system implementation [22].The detection of the faulty sensor initiates two changes in the NEKF.First, the process noise matrix is modified.The ratio of the process noise Q to the measurement noise R is reduced to significantly favor the dynamic state estimate over the residual.This reduces the effect of the measurements from the poor sensor on the system estimate.The process noise of the weights is increased to favor the residual over the weight estimates so that the weights will train to learn the error function.A similar approach of reducing the ratio of Q to R could be used for the reduction of state-coupling error [7,8].The second modification is that, for the first n steps of the correction of the NEKF, the dynamic state estimates are decoupled from the control loop.The resulting modified NEKF equations are given as where the superscript acc indicates the accurate covariance from the estimator using the other sensors.If more than one poor measurement is produced between accurate measurements, only the last measurement was used for the state estimator [7].
Two approaches for decoupling have been considered.In the first, the measurement noise for the faulty sensor is kept artificially high throughout the experiment.This reduces the effect of the poor sensor even as the neural network trains.In the second approach, as the weights of the neural network settle, the measurement noise is reduced from its artificially high values to values closer to the calibrated sensor noise covariance.This allows the improved model of the sensor to have greater effect on the control loop.

Control Example I
A simulated control example is used to demonstrate the capability of the NEKF technique to model a sensor change while the system is in operation.A small motorized vehicle is operating in a two dimensional space, as seen in Figure 1.
The goal is for the vehicle to move from one point in the operational grid to a second point in the operational grid.The vehicle maintains its position via a sensor system that provides a range-bearing measurement to a surveyed location in the operational grid.The platform also has a speedometer and compass system that is used to provide the vehicle's heading relative to a local magnetic pole.This is similar to INS systems that are GPS denied.
The state-space model of the vehicle-dynamics is defined as where a indicates acceleration, θ denotes the heading angle, and ψ denotes the change in heading angle.For a sample rate of dt, the discretized representation becomes The speed sensor and heading sensor have combined into a single sensor package modeled in (23).The package has an update rate of 0.2 seconds.The accuracy of the compass heading is 2.0 degrees while speedometer accuracy is 0.001 m/s.The position sensor, providing a range (r), and bearing (α) relative to the surveyed location, is modeled in (24), also with an update rate of 0.2 seconds.The position sensor reports are interleaved with the velocity measurements, as shown in the time line of Figure 2. The accuracy of the position sensor is given as 1.0 degrees for bearing and 0.005 m for range: For this effort, the control input has an input rate of 0.1 seconds to synchronize with the sensor reports.The initial location of the vehicle is defined at (−5 m, −6 m) with a heading of 0 degrees, while the desired final point is (0 m, 0 m) with a heading of 45.0 degrees.The rangebearing beacon is placed at (+3 m, −8 m), while the local north pole is defined at (−2 m, +3 m).The position sensor measurement uncertainty matrix had standard deviations of 0.005 m for the range and 1.0 radian for the bearing.For the velocity sensor, the measurement error covariance had standard deviations of 0.001 m/s for the speed and 2 radians for the heading.The broken velocity sensor reported speeds increased by a factor of 1.9 and added a 0.5-radian bias to the heading, in addition to the additive Gaussian noise.For the NEKF, the process noise for the weights of the neural networks, Q w , and its initial error covariance, P wts , were increased to allow for changes based on the residuals using the broken sensor.The process noise for the input weights was set to 1.0 and for the output weights, it was set to 2.0.The initial state error covariance, P states , was set to 100.0.The process noise, Q, was set to the integrated white noise model [15]: with a factor q of 0.0017.For the NEKF, the initial value for P wts was set to 1000.For comparison purposes, an EKF using the same values for the dynamic state components was generated as well.Four separate cases, two with the EKF and two with the NEKF were implemented.The four cases are (1) an EKF with an inflation factor of 100 throughout the scenario, (2) an EKF with an inflation factor of 10,000,000 throughout the scenario, (3) an NEKF with an inflation factor of 100 throughout the scenario, and (4) an NEKF with an initial inflation factor of 1000 held for 80 iterations and then decayed by 15% for later iterations down to a minimum value of 1.
Inflating the uncertainty matrix, R, of the velocity sensor by a given inflation factor allows the state update component of EKF and of the NEKF to be less affected by these poor measurements.For the NEKF, a 4-node hidden layer was used.The results are shown in Figures 3(a)-3(d).
Figure 3(a) indicates that the EKF is unable to remain stable with the poor measurements having the reduced, but not completely insignificant, effect on the state estimate.By overcompensating with a significant increase in the measurement covariance, the EKF basically eliminates all of the poor sensor reports.This is seen in Figure 3(b).However, the vehicle never settles in at the location.The NEKF results are shown in Figures 3(c) and 3(d) where it learns sensor errors while on line to provide clearly improved control performance.Even with the same measurement covariance as the EKF in first case, as in Figure 3(c), the NEKF implementation remains stable.In both Figures 3(c) and 3(d), the results are quite similar with more changes after the measurement noise decays.
With multiple sensor systems, the NEKF is able to provide a fault correcting mechanism for sensors that are still providing information, although that information needs correction.The research of this effort has also shown that the slower the decay rate is on the inflation factor, the lower the initial inflation factor can be.

Control Example II
The small motorized vehicle operating in a two-dimensional space shown in Figure 1 is also the basis for the second control example.In this case, the vehicle maintains its position via a two-sensor system.One sensor provides a range-bearing measurement to a surveyed location in the operational grid and another, slower reporting, position sensor provides a linear position report.This would be similar to a GPS system updating an INS.
The state-space model of the vehicle-dynamics is defined as in ( 20)-( 22).The range-bearing error is described as in (23).The position sensor has an update rate of 0.6 seconds and provides a linear report as seen in ( 26).The accuracy of the position sensor is assumed to be ±0.01 m in both directions.The other position sensor, providing a range (r), and bearing (α) relative to the surveyed location, is modeled in (24) and has an update rate of 0.2 seconds.The accuracy of the faster, range-bearing position sensor is given as 1.0 degree of bearing accuracy and 0.005 m of range accuracy: The sensor reports of positions are interleaved as shown in the time line of Figure 4.For this effort, the control input has an input rate of 0.2 seconds to synchronize with the rangebearing sensor report.The initial location of the vehicle is defined at (−5 m, −6 m) with a heading of 0 degrees, while the desired final point is (0 m, 0 m) with a heading of 45.0 degrees.The rangebearing beacon is placed at (+3 m, −8 m).The position sensor providing linear reports provides measurements with a bias of (0.5 m, −0.25 m) added to the coordinates and is considered to be miscalibrated.
For the NEKF, the process noise for the weights of the neural networks, Q w , and its initial error covariance, P wts , were increased to allow for changes based on the residuals using the broken sensor.The process noise for the input weights was set to 1.0 and for the output weights, it was set to 2.0.The initial state error covariance, P states , was set to 10.0.The process noise, Q, was set to the integrated white noise model [14] of (25) with a factor q of 0.0017.For the NEKF, the initial value for P wts was set to 0.1.The inflation factor for the broken sensor in both the NEKF and the EKF tests was set to 100.For comparison purposes, the EKF that was generated used the same values for the dynamic state components.
In this example case, when the system had two fully functional sensors, the vehicle took 3.6 seconds to reach its desired endpoint with a 0.001 m total distance error.A comparison between the EKF with the bad sensor and the NEKF with the bad sensor was generated.Two different comparisons were run.In the first comparison, an inflation factor of 10 for measurement error covariance matrix of the broken sensor was used.Inflating the uncertainty matrix, R, of the velocity sensor by a given inflation factor allows the state update component of EKF and of the NEKF to be less affected by these poor measurements.For the NEKF, a 4node hidden layer was used.In this case, both the NEKF and the EKF went unstable.
In the second comparison, an inflation factor of 1000 was used.The results of the four states, x-position, xvelocity, y-position, and y-velocity are shown for the EKF in Figure 5.While the individual elements are hard to discern, the important fact is that none of the values converge.This causes the vehicle to leave the region of interest.Figure 6 depicts the NEKF results for the four states.As is clearly seen, both techniques achieve a result, but the NEKF trajectory is much smoother and the states are less erratic and require significantly fewer iterations to converge to the desired result (84 iterations versus 498 iterations).
If the position sensor was miscalibrated and not corrected but only had a reduced effect on the navigation estimator, as would be the case when using the EKF only and assumed it were inaccurate, the vehicle position becomes unstable causing it to leave the grid area within 10 seconds of beginning the test.When the NEKF was used, the vehicle was able to reach its desired endpoint in 38.4 seconds with 0.45 m total distance error and 0.3 m/s velocity error.One of the interesting comparisons is that of the residual errors for heading in this example compared to a 3-sigma error based upon the system and sensor covariances.Figure 7 shows the results for the EKF, and Figure 8 shows the results for the NEKF.In both cases, the residuals are within the bounds, but the NEKF has a more accurate bound in the steady-state portion allowing for the Kalman gain to permit greater residual effect on the state update for the corrected sensor.Thus, without the NEKF, the system would go unstable with deleterious effects almost immediately.With the NEKF, the vehicle remains in the operational area and slowly converges to the desired point.

Conclusion
A new neural extended Kalman filter algorithm has been developed that can improve sensor modeling for a poorly modeled sensor in the control loop.The technique expanded on an approach originally developed for target tracking that relied upon that availability of truth.In the algorithm, properly modeled, operating sensors were used to provide truth used to recalibrate the sensor that is poorly modeled.This NEKF approach decouples its state estimates from the accurate estimates using the measurement error covariance.
The NEKF was shown to have improved performance over that of the EKF in its application to estimate the states of the control loop of an autonomous vehicle for two example cases.Performance of the algorithm will continue to be evaluated in other applications and with different training parameters.

Figure 1 :
Figure 1: Example scenario of an autonomous vehicle using two sensors to achieve a specific end location.

Figure 2 :
Figure 2: Sensor reports from different sensors are interleaved to provide uniform reporting.

Figure 3 :
Figure 3: (a) Position coordinates using the EKF and a low inflation rate.(b) Position coordinates using the EKF and a high inflation rate.(c)Position coordinates using the NEKF and a low inflation rate.(d) Position coordinates using the NEKF and a higher inflation rate that is allowed to decay after 80 iterations.

Figure 4 :
Figure 4: Sensor reports from different sensors are interleaved to provide uniform reporting.

20 Figure 7 :Figure 8 :
Figure 7: Comparison of EKF residual and the error covariance weighting.