Nonlinear Unknown Input and State Estimation Algorithm in Mobile Robots
Technical Report No. Cyber-Security-Lab-2018-001
This technical report provides the description and the derivation of a novel nonlinear unknown input and state estimation algorithm (NUISE) for mobile robots. The algorithm is designed for real-world robots with nonlinear dynamic models and subject to stochastic noises on sensing and actuation. Leveraging sensor readings and planned control commands, the algorithm detects and quantifies anomalies on both sensors and actuators. Later, we elaborate the dynamic models of two distinctive mobile robots for the purpose of demonstrating the application of NUISE. This report serves as a supplementary document for [dsn_paper].
Keywords: robotics, estimation theory, anomaly detection, dynamic model
I NUISE Algorithm and its Derivation
Minimum variance unbiased state and unknown input estimation is first introduced in [kitanidis1987unbiased] with indirect feedthrough111Indirect feedthrough suggests that the input of a system indirectly influences the output through system states change. Direct feedthrough suggests that the input of a system is directly connected/fed to the output. unknown input. The method has been extended by many research studies. A general parameterized gain matrix is derived in [darouach1997unbiased]. Estimation with direct feedthrough unknown input is proposed in [cheng2009unbiased, hou1998optimal]. Young et al. [SY-MZ-EF:Automatica16] analyze the stability of systems with direct and indirect feedthrough unknown input. Estimators with indirect feedthrough unknown input has been applied to the fault detection in systems without noise [chen1996design] and with noises [de1997nonparametric, liu2011robust]. An estimator with both direct and indirect feedthrough unknown input is proposed [yong2016simultaneous] for the attack detection in systems with noises, where the attack location is unknown.
One limitation of the aforementioned works is that the proposed methods are limited to handle linear systems. An estimator that can handle nonlinear systems is unexplored. In this work, we propose the nonlinear unknown input and state estimation algorithm (NUISE) as an extension of the above references for nonlinear systems. The algorithm can also be viewed as an extension of the extended Kalman filters [jazwinski2007stochastic] for state estimation of nonlinear systems by integrating unknown input estimation. It is the first time to study the state and unknown input estimation problem in stochastic nonlinear systems. Leveraging the reference sensor readings and planned control commands from the last iteration, NUISE estimates new robot states, corruptions in testing sensor readings, corruptions in control commands, and a likelihood for each mode.
Algorithm 1 describes the complete NUISE algorithm. We first present the definition of optimal estimates in an estimation problem. Optimal estimates contain two properties. Firstly, the estimates are unbiased, i.e., its expected value is equal to the targeted value. Secondly, the estimates have a minimum error covariance matrix, i.e., the estimation error variances are minimized with the given information.
We derive the NUISE algorithm in 4 steps: 1) actuator anomaly vector estimation, 2) state prediction, 3) state estimation, and 4) testing sensor anomaly vector estimation. In each intermediate step, the estimation errors and covariance matrices are calculated accordingly in order to find the optimal estimates.
Consider a particular mode of the dynamic model (2) in [dsn_paper] with potential robot misbehaviors
where vector and represent sensor anomaly vector and actuator anomaly vector, respectively. In mode , testing sensor readings might be modified by anomaly vector . Reference sensor readings are assumed to be clean. We omit mode index in the remaining part of the NUISE derivation for the ease of presentations. The dynamic system (1) can be linearized into
Actuator anomaly vector estimation: Given unbiased estimates of previous states , we can predict the current states using the known kinematic function as follows
The estimation error is described as
Noticeably, the estimation is biased, i.e., , because we did not consider the possible unknown misbehaviors yet, i.e., . To obtain an unbiased state prediction, we needed to find the estimates of the actuator anomaly. The expected output without considering actuator misbehaviors is . The discrepancy between what we expected and what we actually obtain indicates the impact of actuator anomaly . Therefore, the actuator anomaly vector estimates can be obtained linearly from the sensor output bias
where the estimator gain represents a weighted average of the sensor bias. The unknown input estimates are unbiased, i.e., providing that , and . In order to achieve optimal estimates, matrix gain should be carefully chosen with minimum variances. To do this, consider the sensor output bias
where and its covariances are calculated by
where . We choose the matrix using the Gauss Markov theorem [kailath2000linear]
which satisfies . We assume that is invertible. Anomaly vector estimation error covariances are .
State prediction: Estimates are calculated under a partial knowledge of misbehaviors. Since we have the actuator anomaly estimates from the previous step, we can update the state estimates
The state estimates are now unbiased, i.e., , since . Now we find the state prediction error covariance matrix
where and .
State estimation: Predicted states are not perfect because of process and measurement noises. In order to obtain the estimates accurately considering noises, we do corrections on the state estimates using sensor readings. We utilize the discrepancy between the newly predicted outputs and the reference sensor outputs as an indication of the impact of unknown noises
where the state estimates are unbiased, i.e., , and the estimate gain matrix will be chosen such that the new estimates have the smallest error variances. Error dynamic and covariances are
To achieve optimal estimation, we solve the variance minimization problem: . We take the derivative of the objective function with respect to the decision variable and set it as zero
where must be invertible.
Testing sensor anomaly vector estimation: Given , the linear estimation for unknown sensor anomaly vector can be
where the estimates are unbiased, i.e., , providing that . This also can be found by Gauss Markov theorem. By the theorem, the optimal estimates are
where . The covariance matrices can be obtained by
Likelihood of a mode: In order to determine the ground truth condition of a robot, i.e., mode, we calculate a likelihood that reflects the discrepancy between the predicted output and the measured output of a mode. For , we quantify the discrepancy between the predicted output and the measured output as follows
We approximate the output error as a multivariate Gaussian random variable. Then, the likelihood function is given by
where is the error covariance matrix of and . Notations and refer to pseudoinverse and pseudodeterminant, respectively. By the Bayes’ theorem, the a posteriori probability is . However, such updates might cause the of certain modes to converge to zero. To prevent this, we modify the posterior probability update to the following
where , and is a pre-selected small constant preventing the vanishment of the mode probability. The last step is to generate estimates of states and anomaly vector estimates of the maximum a posteriori mode.
Ii Khepera Dynamic Model
Kinematic model The kinematic model of Khepera includes three states: is the robot location at a 2-D plane, and is its heading. The control commands are specified by two variables: and , which are the speeds of the left and right wheels, respectively. Considering actuator misbehaviors with anomaly vector on the left and right wheel, the kinematic model can be presented as
where is assumed to be zero mean Gaussian process noises, and is the distance between the left and right wheel on the chassis of Khepera.
Measurement model The sensor readings include sensing data from three sensors: where is from the IPS, is from the wheel encoder, and is from the LiDAR.
IPS sensor directly measures the states of Khepera, hence, the measurement model can be directly specified by
where refers to measurement noises from the IPS sensor, and refers to the sensor anomaly vector on IPS.
The raw data measured by the wheel encoder are the distances traveled by each wheel in a control iteration. For convenience reasons, we convert them into robot states using previous states before we feed the data to the planner
Analogously with IPS, the measurement model for the wheel encoder can be specified as
after the conversion, where refers to measurement noises from the wheel encoder and refers to the sensor anomaly vector on the wheel encoder.
The LiDAR sensor is placed on top of the robot with a shift distance of from the origin as shown in the left plot of Figure 1. Raw sensor readings returned from LiDAR are the distances between LiDAR and the surrounding walls (see the right plot of Figure 1). Given the LiDAR readings, we process the raw data into the perpendicular distance from each boundary wall and the orientation of Khepera. Specifically, we recognize the straight line segments using raw distances from all direction, and calculate the distances to each wall as follows
where refers to measurement noises from LiDAR. The distance and the angle of each wall in the global coordinate is known in advance as the map information. Using of each wall and the 240 degrees of range, we can also infer the angle of the robot. We use the distance and the angle to each wall as the sensor readings from LiDAR: . In outdoor environments, LiDAR measurement model can be obtained using more complicated simultaneous localization and mapping (SLAM) algorithms [durrant2006simultaneous]. For demonstration purposes, we apply a simple transformation in the indoor environment [jetto1999development].
Iii Tamiya RC Car Dynamic Model
Kinematic model The kinematic model of a Tamiya RC car is presented in Figure 2. The states of the vehicle also include the location and the orientation in a 2D plane. The control includes the longitudinal velocity and the steering . The kinematic model of the vehicle can be described as
where is assumed to be a zero mean Gaussian process noise vector, is the actuator anomaly vector, is the wheelbase, and is the control iteration interval.
Measurement model At each instant of time, sensor readings include data from three sensors: , where each vector refers to the sensor readings from IPS, LiDAR, and IMU, respectively. The measurement models for IPS and LiDAR are similar to those in Khepera (see Section II).
The IMU sensor generates a quaternion , a 3-D acceleration , and a 3-D rotational speed on a body-fixed coordinate. We first obtain the coordinate transformation matrix from the body-fixed coordinate to the global coordinate [kuipers1999quaternions].
The acceleration vector and the rotation speed on the global coordinate system can be obtained as and , respectively. The vehicle velocity vector can be updated by: . Then the state vector can be calculated by integration as follows
Iv Separating Actuator Anomaly Vector
In Section IV.D. of [dsn_paper], we mention that RoboADS only checks the aggregate test statistics instead of each individual actuator. This section explains the reason in detail.
At a high level, the actuator anomaly vectors are statistically correlated. Without loss of generosity, we consider a robot with two actuators such as Khepera. During actuator anomaly vector estimation, we obtain , with error covariances . In Algorithm 1 line 20, we test
to determine the existence of actuator misbehaviors. The threshold is a Chi-square test value with the degree of freedom and the confidence level .
In order to confirm actuator misbehaviors on each actuator, we need to separately conduct Chi-square test , and , with corresponding marginal variances , and :