Multiple Autonomous Robotic Systems Laboratory Technical Report ...
Observability-constrained EKF Implementation of the IMU-RGBD Camera Navigation using Point and Plane Features
Chao X. Guo and Stergios I. Roumeliotis
Multiple Autonomous Robotic Systems Laboratory Technical Report Number -2013-001 March 2013 Repository Revision: Last Update: Dept. of Computer Science & Engineering University of Minnesota 4-192 EE/CS Building 200 Union St. S.E. Minneapolis, MN 55455 Tel: (612) 625-2217 Fax: (612) 625-0572 URL: http://mars.cs.umn.edu
Observability-constrained EKF Implementation of the IMU-RGBD Camera Navigation using Point and Plane Features Chao X. Guo and Stergios I. Roumeliotis Multiple Autonomous Robotic Systems Laboratory, TR-2013-001
March 2013
I. A LGORITHM D ESCRIPTION In this report, we present our IMU-RGBD camera navigation algorithm employing the observability constrained (OC)-EKF, which seeks to maintain the original system’s observability properties in the linearized implementation (EKF). In particular, we first describe the implementation of OC-EKF for processing point feature measurements. Then, we prove that once the OC-EKF is employed for point feature measurements, the observability constraint is automatically satisfied for the plane feature measurements. A system’s observability Gramian [1], M, is defined as H1 H2 Φ2,1 M= . (1) .. Hk Φk,1 where Φk,1 , Φk−1 · · · Φ1 is the state transition matrix from time step 1 to k, and Hk is the measurement Jacobian at time step k. As described in [1], a system’s unobservable directions, N, are supposed to span the observability Gramian’s nullspace MN = 0
(2)
However, in [2] and [3], the authors show that (2) does not hold when a nonlinear system is linearized with the current state estimate, due to linearization errors. Therefore, the EKF gains spurious informations along unobservable directions, which results in smaller uncertainty (that causes the filter to be inconsistent) and larger estimation errors. To address this problem, the OC-EKF modifies the state transition and measurement Jacobian matrices in such a way so that the resulting linearized system adheres to the observability properties of the original nonlinear system. In particular, in [2] it is proved that (2) can be satisfied by enforcing the following two observability constraints: Nk+1 = Φk Nk Hk Nk = 0,
∀k > 0
(3) (4)
where Nk and Nk+1 are the unobservable directions evaluated at time-steps k and k + 1. Hereafter, we will show how to appropriately modify Φk and Hk so as to satisfy the constraints (3) and (4), thus retain the system’s observability properties. A. Observability Constraint for Point Feature Measurements In this section, we present the implementation of the OC-EKF for the IMU-RGBD camera navigation system using point feature measurements. (1) Modification of the State Transition Matrix Φk : We start by modifying the state transition matrix, Φk , according to the observability constraint (3) k k Nk+1 point = Φ N point
(5)
where Nkpoint and Nk+1 point are the nullspace for the IMU-RGBD camera navigation system when using only point features at time-steps k and k + 1 respectively, which is defined as N point
∂s Cg ∂θ
03 03 I3 = Ng point I3 03 03
−bvcg , −bpcg −bp f cg 0 3 03
in [5], and Φk has the following structure: Φ11 Φ21 Φ31 03 03 03
03 I3 δtI3 03 03 03
03 03 I3 03 03 03
k+1
03 03 03 I3 03 03
03 Φ25 Φ35 03 I3 03
p
N point
Φ16 Φ26 Φ36 03 03 I3
(6)
(7)
k
Substituting N point and Φk into (5), N ppoint = Φk N ppoint is automatically satisfied because all the block rows result k+1 k in 03 = 03 or I3 = I3 . However, to fulfill constraint Ngpoint = Φk Ngpoint , we need to modify Φk so as to satisfy the following three constraints: Φ11 Ck g = Ck+1 g
(8)
Φ21 Ck g = bvk cg − bvk+1 cg
(9)
Φ31 Ck g = δtbvk cg + bpk cg − bpk+1 cg
(10)
where Φ11 , Φ21 , Φ31 are 3 × 3 matrices in the first block column of the first, second and third block rows of state T transition matrix Φk . Constraint (8) can be simply satisfied by modifying Φ∗11 = Ck+1 Ck . Both of (9) and (10) are in the form Au = w, where u and w comprise nullspace elements that are fixed, and we seek to select another matrix A∗ that is closest to the original matrix A in the Frobenius norm sense, while satisfying constraints (9) and (10). To do so, we formulate the following optimization problem A∗ = argmin ||A∗ − A||2F
(11)
A∗
s. t. A∗ u = w where || · ||F denotes the Frobenius matrix norm. The optimal A∗ can be determined by solving its KKT optimality condition [?], whose solution is A∗ = A − (Au − w)(uT u)−1 uT
(12)
(2) Modification of the Measurement Jacobian Hkpoint : During the update, we seek to modify the Jacobian matrix so as to fulfill constraint (4), i.e.,
Hkpoint
Hkpoint Nkpoint = 0
(13)
where Hkpoint is the measurement Jacobian matrix for point features evaluated as time step k, defined as H point = Hθ2 03×3 Hp Hp f 03×6
(14)
where Hθ2 = Hp =
TR-2013-001. RepRev
∂ z point = bC(I qˆ G )(G pˆ f − G pˆ I )c ∂ I θG
∂ z point = −C(I qˆ G ), ∂ Gp
Hp f =
∂ z point = C(I qˆ G ) ∂ Gp f 3
and z point is the measurement model for point features with noise η point : z point = I p f + η point = C(I qG )(G p f − G pI ) + η point
(15)
Substituting Hkpoint and Nkpoint , into (13), it can be shown that (13) is equivalent to the following two constraints " # h i Ck g k k Hθ2 Hp =0 (16) bpkf c − bpk c g Hkp f = Hkp
(17)
(16) is a constraint of the form Au = 0, where u is a fixed quantity determined by elements in the nullspace, and A comprises elements of the measurement Jacobian Hkpoint . We compute the optimal A∗ that satisfies this relationship ∗ ∗ using (11) and (12) for the special case when w = 0, and select Hkp f = Hkp . B. Observability Constraint for Plane Feature Measurements In this section, we will prove that once the OC-EKF is applied to the IMU-RGBD camera navigation system using point feature measurements, the observability constraint (2) is automatically satisfied for the plane feature measurements. The measurement Jacobian for plane features, H plane , is computed using the chain rule as: H plane = Hc Hθ1 03×15 (18) where Hc = Hθ1
∂ z0plane
zˆ plane (3) 0 −ˆz plane (1) = 0 zˆ plane (3) −ˆz plane (2) zˆ plane (3)2 1
∂ In ∂ In = = bC(I qˆ G )G nc ∂ I θˆG
(19)
and z0plane is the measurement model for plane features of known normal vector, G n, with noise C(ηθ ): z (1)/z plane (3) z0plane = plane z plane (2)/z plane (3) z plane = C(ηθ )I n = C(ηθ )C(I qG )G n
(20) (21)
Substituting Φk and Hkplane into the observability Gramian M plane for plane feature measurements, the first block row of M plane is just the measurement Jacobian matrix M plane (1) = H1plane = H1c bC(I q1G )G nc 03×15 (22) and the kth block row is computed as M plane (k) = Hkplane Φk,1 = Hkplane Φk−1 · · · Φ1 = Hkc bC(I qkG )G ncΠk 03×12 Ψk 1 k where Πk = Φk−1 11 · · · Φ11 , and Ψ is a time-varying matrix that does not affect the current analysis. When applying T the OC-EKF to point features, we have modified Φk11 = Ck+1 Ck . Therefore, multiplying Ngplane and N pplane to the right hand side of M plane , we have:
M plane (1)Ngplane = H1c bC(I q1G )G ncC(I q1G )G n = 0 1 I 1 G M plane (k)Ngplane = Hkc bC(I qkG )G ncΦk−1 11 · · · Φ11 C( qG ) n T
T
= Hkc bC(I qkG )G ncCk Ck−1 · · · C2 C1 C(I q1G )G n = Hkc bC(I qkG )G ncC(I qkG )G n = 0 M plane (1)N pplane = M plane (k)N pplane = 0 Thus, M plane N plane = 0 is automatically satisfied. In summary, after enforcing the observability constraint for point TR-2013-001. RepRev
4
feature measurements on the state transition matrix, Φk , the observability constraint (2) is automatically satisfied for the plane feature measurements. R EFERENCES [1] P. S. Maybeck, Stochastic models, estimation and control. New York, NY: Academic Press, 1979, vol. 1. [2] J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, “Observability-constrained vision-aided inertial navigation,” University of Minnesota, Dept. of Comp. Sci. & Eng., MARS Lab, Tech. Rep., Feb. 2012. [Online]. Available: http: //www-users.cs.umn.edu/∼joel/index.php?page=publications [3] ——, “Towards consistent vision-aided inertial navigation,” in the 10th International Workshop on the Algorithmic Foundations of Robotics, Cambridge, Massachusetts, June 13–15 2012. [4] C. X. Guo and S. I. Roumeliotis, “Observability-constrained EKF implmentation of the IMU-RGBD camera navigation using point and plane features,” University of Minnesota, Tech. Rep., Mar. 2013. [Online]. Available: http://www-users.cs.umn.edu/∼chaguo/ [5] ——, “IMU-RGBD camera 3d pose estimation and extrinsic calibration: Observability analysis and consistency improvement,” in Proc. of the IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, May 6–10 2013, (to appear).
TR-2013-001. RepRev
5
Chao X. Guo and Stergios I. Roumeliotis
Multiple Autonomous Robotic Systems Laboratory Technical Report Number -2013-001 March 2013 Repository Revision: Last Update: Dept. of Computer Science & Engineering University of Minnesota 4-192 EE/CS Building 200 Union St. S.E. Minneapolis, MN 55455 Tel: (612) 625-2217 Fax: (612) 625-0572 URL: http://mars.cs.umn.edu
Observability-constrained EKF Implementation of the IMU-RGBD Camera Navigation using Point and Plane Features Chao X. Guo and Stergios I. Roumeliotis Multiple Autonomous Robotic Systems Laboratory, TR-2013-001
March 2013
I. A LGORITHM D ESCRIPTION In this report, we present our IMU-RGBD camera navigation algorithm employing the observability constrained (OC)-EKF, which seeks to maintain the original system’s observability properties in the linearized implementation (EKF). In particular, we first describe the implementation of OC-EKF for processing point feature measurements. Then, we prove that once the OC-EKF is employed for point feature measurements, the observability constraint is automatically satisfied for the plane feature measurements. A system’s observability Gramian [1], M, is defined as H1 H2 Φ2,1 M= . (1) .. Hk Φk,1 where Φk,1 , Φk−1 · · · Φ1 is the state transition matrix from time step 1 to k, and Hk is the measurement Jacobian at time step k. As described in [1], a system’s unobservable directions, N, are supposed to span the observability Gramian’s nullspace MN = 0
(2)
However, in [2] and [3], the authors show that (2) does not hold when a nonlinear system is linearized with the current state estimate, due to linearization errors. Therefore, the EKF gains spurious informations along unobservable directions, which results in smaller uncertainty (that causes the filter to be inconsistent) and larger estimation errors. To address this problem, the OC-EKF modifies the state transition and measurement Jacobian matrices in such a way so that the resulting linearized system adheres to the observability properties of the original nonlinear system. In particular, in [2] it is proved that (2) can be satisfied by enforcing the following two observability constraints: Nk+1 = Φk Nk Hk Nk = 0,
∀k > 0
(3) (4)
where Nk and Nk+1 are the unobservable directions evaluated at time-steps k and k + 1. Hereafter, we will show how to appropriately modify Φk and Hk so as to satisfy the constraints (3) and (4), thus retain the system’s observability properties. A. Observability Constraint for Point Feature Measurements In this section, we present the implementation of the OC-EKF for the IMU-RGBD camera navigation system using point feature measurements. (1) Modification of the State Transition Matrix Φk : We start by modifying the state transition matrix, Φk , according to the observability constraint (3) k k Nk+1 point = Φ N point
(5)
where Nkpoint and Nk+1 point are the nullspace for the IMU-RGBD camera navigation system when using only point features at time-steps k and k + 1 respectively, which is defined as N point
∂s Cg ∂θ
03 03 I3 = Ng point I3 03 03
−bvcg , −bpcg −bp f cg 0 3 03
in [5], and Φk has the following structure: Φ11 Φ21 Φ31 03 03 03
03 I3 δtI3 03 03 03
03 03 I3 03 03 03
k+1
03 03 03 I3 03 03
03 Φ25 Φ35 03 I3 03
p
N point
Φ16 Φ26 Φ36 03 03 I3
(6)
(7)
k
Substituting N point and Φk into (5), N ppoint = Φk N ppoint is automatically satisfied because all the block rows result k+1 k in 03 = 03 or I3 = I3 . However, to fulfill constraint Ngpoint = Φk Ngpoint , we need to modify Φk so as to satisfy the following three constraints: Φ11 Ck g = Ck+1 g
(8)
Φ21 Ck g = bvk cg − bvk+1 cg
(9)
Φ31 Ck g = δtbvk cg + bpk cg − bpk+1 cg
(10)
where Φ11 , Φ21 , Φ31 are 3 × 3 matrices in the first block column of the first, second and third block rows of state T transition matrix Φk . Constraint (8) can be simply satisfied by modifying Φ∗11 = Ck+1 Ck . Both of (9) and (10) are in the form Au = w, where u and w comprise nullspace elements that are fixed, and we seek to select another matrix A∗ that is closest to the original matrix A in the Frobenius norm sense, while satisfying constraints (9) and (10). To do so, we formulate the following optimization problem A∗ = argmin ||A∗ − A||2F
(11)
A∗
s. t. A∗ u = w where || · ||F denotes the Frobenius matrix norm. The optimal A∗ can be determined by solving its KKT optimality condition [?], whose solution is A∗ = A − (Au − w)(uT u)−1 uT
(12)
(2) Modification of the Measurement Jacobian Hkpoint : During the update, we seek to modify the Jacobian matrix so as to fulfill constraint (4), i.e.,
Hkpoint
Hkpoint Nkpoint = 0
(13)
where Hkpoint is the measurement Jacobian matrix for point features evaluated as time step k, defined as H point = Hθ2 03×3 Hp Hp f 03×6
(14)
where Hθ2 = Hp =
TR-2013-001. RepRev
∂ z point = bC(I qˆ G )(G pˆ f − G pˆ I )c ∂ I θG
∂ z point = −C(I qˆ G ), ∂ Gp
Hp f =
∂ z point = C(I qˆ G ) ∂ Gp f 3
and z point is the measurement model for point features with noise η point : z point = I p f + η point = C(I qG )(G p f − G pI ) + η point
(15)
Substituting Hkpoint and Nkpoint , into (13), it can be shown that (13) is equivalent to the following two constraints " # h i Ck g k k Hθ2 Hp =0 (16) bpkf c − bpk c g Hkp f = Hkp
(17)
(16) is a constraint of the form Au = 0, where u is a fixed quantity determined by elements in the nullspace, and A comprises elements of the measurement Jacobian Hkpoint . We compute the optimal A∗ that satisfies this relationship ∗ ∗ using (11) and (12) for the special case when w = 0, and select Hkp f = Hkp . B. Observability Constraint for Plane Feature Measurements In this section, we will prove that once the OC-EKF is applied to the IMU-RGBD camera navigation system using point feature measurements, the observability constraint (2) is automatically satisfied for the plane feature measurements. The measurement Jacobian for plane features, H plane , is computed using the chain rule as: H plane = Hc Hθ1 03×15 (18) where Hc = Hθ1
∂ z0plane
zˆ plane (3) 0 −ˆz plane (1) = 0 zˆ plane (3) −ˆz plane (2) zˆ plane (3)2 1
∂ In ∂ In = = bC(I qˆ G )G nc ∂ I θˆG
(19)
and z0plane is the measurement model for plane features of known normal vector, G n, with noise C(ηθ ): z (1)/z plane (3) z0plane = plane z plane (2)/z plane (3) z plane = C(ηθ )I n = C(ηθ )C(I qG )G n
(20) (21)
Substituting Φk and Hkplane into the observability Gramian M plane for plane feature measurements, the first block row of M plane is just the measurement Jacobian matrix M plane (1) = H1plane = H1c bC(I q1G )G nc 03×15 (22) and the kth block row is computed as M plane (k) = Hkplane Φk,1 = Hkplane Φk−1 · · · Φ1 = Hkc bC(I qkG )G ncΠk 03×12 Ψk 1 k where Πk = Φk−1 11 · · · Φ11 , and Ψ is a time-varying matrix that does not affect the current analysis. When applying T the OC-EKF to point features, we have modified Φk11 = Ck+1 Ck . Therefore, multiplying Ngplane and N pplane to the right hand side of M plane , we have:
M plane (1)Ngplane = H1c bC(I q1G )G ncC(I q1G )G n = 0 1 I 1 G M plane (k)Ngplane = Hkc bC(I qkG )G ncΦk−1 11 · · · Φ11 C( qG ) n T
T
= Hkc bC(I qkG )G ncCk Ck−1 · · · C2 C1 C(I q1G )G n = Hkc bC(I qkG )G ncC(I qkG )G n = 0 M plane (1)N pplane = M plane (k)N pplane = 0 Thus, M plane N plane = 0 is automatically satisfied. In summary, after enforcing the observability constraint for point TR-2013-001. RepRev
4
feature measurements on the state transition matrix, Φk , the observability constraint (2) is automatically satisfied for the plane feature measurements. R EFERENCES [1] P. S. Maybeck, Stochastic models, estimation and control. New York, NY: Academic Press, 1979, vol. 1. [2] J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, “Observability-constrained vision-aided inertial navigation,” University of Minnesota, Dept. of Comp. Sci. & Eng., MARS Lab, Tech. Rep., Feb. 2012. [Online]. Available: http: //www-users.cs.umn.edu/∼joel/index.php?page=publications [3] ——, “Towards consistent vision-aided inertial navigation,” in the 10th International Workshop on the Algorithmic Foundations of Robotics, Cambridge, Massachusetts, June 13–15 2012. [4] C. X. Guo and S. I. Roumeliotis, “Observability-constrained EKF implmentation of the IMU-RGBD camera navigation using point and plane features,” University of Minnesota, Tech. Rep., Mar. 2013. [Online]. Available: http://www-users.cs.umn.edu/∼chaguo/ [5] ——, “IMU-RGBD camera 3d pose estimation and extrinsic calibration: Observability analysis and consistency improvement,” in Proc. of the IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, May 6–10 2013, (to appear).
TR-2013-001. RepRev
5
