diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml
index 7b13b6646..fa2425e4d 100644
--- a/.github/workflows/build-linux.yml
+++ b/.github/workflows/build-linux.yml
@@ -20,26 +20,26 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          ubuntu-18.04-gcc-5,
-          ubuntu-18.04-gcc-9,
-          ubuntu-18.04-clang-9,
+          ubuntu-20.04-gcc-7,
+          ubuntu-20.04-gcc-9,
+          ubuntu-20.04-clang-9,
         ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-          - name: ubuntu-18.04-gcc-5
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-7
+            os: ubuntu-20.04
             compiler: gcc
-            version: "5"
+            version: "7"
 
-          - name: ubuntu-18.04-gcc-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-9
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
 
-          - name: ubuntu-18.04-clang-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-clang-9
+            os: ubuntu-20.04
             compiler: clang
             version: "9"
 
@@ -60,9 +60,9 @@ jobs:
             gpg -a --export $LLVM_KEY | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi
-          sudo apt-get -y update
 
-          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y update
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml
index 1e1519da6..4eb861ecc 100644
--- a/.github/workflows/build-python.yml
+++ b/.github/workflows/build-python.yml
@@ -19,34 +19,34 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          ubuntu-18.04-gcc-5,
-          ubuntu-18.04-gcc-9,
-          ubuntu-18.04-clang-9,
+          ubuntu-20.04-gcc-7,
+          ubuntu-20.04-gcc-9,
+          ubuntu-20.04-clang-9,
           macOS-11-xcode-13.4.1,
-          ubuntu-18.04-gcc-5-tbb,
+          ubuntu-20.04-gcc-7-tbb,
         ]
 
         build_type: [Debug, Release]
         python_version: [3]
         include:
-          - name: ubuntu-18.04-gcc-5
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-7
+            os: ubuntu-20.04
             compiler: gcc
-            version: "5"
+            version: "7"
 
-          - name: ubuntu-18.04-gcc-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-9
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
 
-          - name: ubuntu-18.04-clang-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-clang-9
+            os: ubuntu-20.04
             compiler: clang
             version: "9"
 
           # NOTE temporarily added this as it is a required check.
-          - name: ubuntu-18.04-clang-9
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-clang-9
+            os: ubuntu-20.04
             compiler: clang
             version: "9"
             build_type: Debug
@@ -57,10 +57,10 @@ jobs:
             compiler: xcode
             version: "13.4.1"
 
-          - name: ubuntu-18.04-gcc-5-tbb
-            os: ubuntu-18.04
+          - name: ubuntu-20.04-gcc-7-tbb
+            os: ubuntu-20.04
             compiler: gcc
-            version: "5"
+            version: "7"
             flag: tbb
 
     steps:
@@ -79,9 +79,9 @@ jobs:
             gpg -a --export $LLVM_KEY | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi
+
           sudo apt-get -y update
-          
-          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev
           
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml
index d357b9a34..ef7d7723d 100644
--- a/.github/workflows/build-special.yml
+++ b/.github/workflows/build-special.yml
@@ -32,31 +32,31 @@ jobs:
 
         include:
           - name: ubuntu-gcc-deprecated
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: deprecated
 
           - name: ubuntu-gcc-quaternions
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: quaternions
 
           - name: ubuntu-gcc-tbb
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: tbb
 
           - name: ubuntu-cayleymap
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: cayley
 
           - name: ubuntu-system-libs
-            os: ubuntu-18.04
+            os: ubuntu-20.04
             compiler: gcc
             version: "9"
             flag: system-libs
@@ -74,9 +74,9 @@ jobs:
             gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi
-          sudo apt-get -y update
 
-          sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
+          sudo apt-get -y update
+          sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
 
           if [ "${{ matrix.compiler }}" = "gcc" ]; then
             sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
diff --git a/README.md b/README.md
index 52ac0a5d8..b32ce70e0 100644
--- a/README.md
+++ b/README.md
@@ -64,6 +64,29 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali
 
 We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.
 
+## Citation
+
+If you are using GTSAM for academic work, please use the following citation:
+
+```
+@software{gtsam,
+  author       = {Frank Dellaert and Richard Roberts and Varun Agrawal and Alex Cunningham and Chris Beall and Duy-Nguyen Ta and Fan Jiang and lucacarlone and nikai and Jose Luis Blanco-Claraco and Stephen Williams and ydjian and John Lambert and Andy Melim and Zhaoyang Lv and Akshay Krishnan and Jing Dong and Gerry Chen and Krunal Chande and balderdash-devil and DiffDecisionTrees and Sungtae An and mpaluri and Ellon Paiva Mendes and Mike Bosse and Akash Patel and Ayush Baid and Paul Furgale and matthewbroadwaynavenio and roderick-koehle},
+  title        = {borglab/gtsam},
+  month        = may,
+  year         = 2022,
+  publisher    = {Zenodo},
+  version      = {4.2a7},
+  doi          = {10.5281/zenodo.5794541},
+  url          = {https://doi.org/10.5281/zenodo.5794541}
+}
+```
+
+You can also get the latest citation available from Zenodo below:
+
+[![DOI](https://zenodo.org/badge/86362856.svg)](https://doi.org/10.5281/zenodo.5794541)
+
+Specific formats are available in the bottom-right corner of the Zenodo page.
+
 ## The Preintegrated IMU Factor
 
 GTSAM includes a state of the art IMU handling scheme based on
@@ -73,7 +96,7 @@ GTSAM includes a state of the art IMU handling scheme based on
 Our implementation improves on this using integration on the manifold, as detailed in
 
 - Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, _"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors"_, Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
-- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
+- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, _"IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation"_, Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
 
 If you are using the factor in academic work, please cite the publications above.
 
diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 9058807ad..e63fbf1dd 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -190,7 +190,7 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
 endif()
 
 if (NOT MSVC)
-  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
+  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
   if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64"))
     # Add as public flag so all dependant projects also use it, as required
     # by Eigen to avid crashes due to SIMD vectorization:
diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake
index b21d16885..c49eb4f8e 100644
--- a/cmake/HandleEigen.cmake
+++ b/cmake/HandleEigen.cmake
@@ -1,7 +1,10 @@
 ###############################################################################
 # Option for using system Eigen or GTSAM-bundled Eigen
-
-option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
+# Default: Use system's Eigen if found automatically:
+find_package(Eigen3 QUIET)
+set(USE_SYSTEM_EIGEN_INITIAL_VALUE ${Eigen3_FOUND})
+option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${USE_SYSTEM_EIGEN_INITIAL_VALUE})
+unset(USE_SYSTEM_EIGEN_INITIAL_VALUE)
 
 if(NOT GTSAM_USE_SYSTEM_EIGEN)
   # This option only makes sense if using the embedded copy of Eigen, it is
@@ -11,7 +14,7 @@ endif()
 
 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
-    find_package(Eigen3 REQUIRED)
+    find_package(Eigen3 REQUIRED) # need to find again as REQUIRED
 
     # Use generic Eigen include paths e.g. <Eigen/Core>
     set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx
index 0922a3e9c..c335e6949 100644
--- a/doc/ImuFactor.lyx
+++ b/doc/ImuFactor.lyx
@@ -1,7 +1,9 @@
-#LyX 2.0 created this file. For more info see http://www.lyx.org/
-\lyxformat 413
+#LyX 2.3 created this file. For more info see http://www.lyx.org/
+\lyxformat 544
 \begin_document
 \begin_header
+\save_transient_properties true
+\origin unavailable
 \textclass article
 \use_default_options true
 \maintain_unincluded_children false
@@ -9,16 +11,18 @@
 \language_package default
 \inputencoding auto
 \fontencoding global
-\font_roman default
-\font_sans default
-\font_typewriter default
+\font_roman "default" "default"
+\font_sans "default" "default"
+\font_typewriter "default" "default"
+\font_math "auto" "auto"
 \font_default_family default
 \use_non_tex_fonts false
 \font_sc false
 \font_osf false
-\font_sf_scale 100
-\font_tt_scale 100
-
+\font_sf_scale 100 100
+\font_tt_scale 100 100
+\use_microtype false
+\use_dash_ligatures true
 \graphics default
 \default_output_format default
 \output_sync 0
@@ -29,16 +33,26 @@
 \use_hyperref false
 \papersize default
 \use_geometry true
-\use_amsmath 1
-\use_esint 1
-\use_mhchem 1
-\use_mathdots 1
+\use_package amsmath 1
+\use_package amssymb 1
+\use_package cancel 1
+\use_package esint 1
+\use_package mathdots 1
+\use_package mathtools 1
+\use_package mhchem 1
+\use_package stackrel 1
+\use_package stmaryrd 1
+\use_package undertilde 1
 \cite_engine basic
+\cite_engine_type default
+\biblio_style plain
 \use_bibtopic false
 \use_indices false
 \paperorientation portrait
 \suppress_date false
+\justification true
 \use_refstyle 1
+\use_minted 0
 \index Index
 \shortcut idx
 \color #008000
@@ -51,7 +65,10 @@
 \tocdepth 3
 \paragraph_separation indent
 \paragraph_indentation default
-\quotes_language english
+\is_math_indent 0
+\math_numbering_side default
+\quotes_style english
+\dynamic_quotes 0
 \papercolumns 1
 \papersides 1
 \paperpagestyle default
@@ -65,11 +82,11 @@
 \begin_body
 
 \begin_layout Title
-The new IMU Factor
+The New IMU Factor
 \end_layout
 
 \begin_layout Author
-Frank Dellaert
+Frank Dellaert & Varun Agrawal
 \end_layout
 
 \begin_layout Standard
@@ -91,6 +108,282 @@ filename "macros.lyx"
 
 \end_layout
 
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
+{\mathfrak{\mathbb{R}^{9}}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rninethree}{\mathfrak{\mathbb{R}^{9\times3}}}
+{\mathfrak{\mathbb{R}^{9\times3}}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rninesix}{\mathfrak{\mathbb{R}^{9\times6}}}
+{\mathfrak{\mathbb{R}^{9\times6}}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rninenine}{\mathfrak{\mathbb{R}^{9\times9}}}
+{\mathfrak{\mathbb{R}^{9\times9}}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsubsection*
+IMU Factor
+\end_layout
+
+\begin_layout Standard
+The IMU factor has 2 variants:
+\end_layout
+
+\begin_layout Enumerate
+ImuFactor is a 5-way factor between the previous pose and velocity, the
+ current pose and velocity, and the current IMU bias.
+\end_layout
+
+\begin_layout Enumerate
+ImuFactor2 is a 3-way factor between the previous NavState, the current
+ NavState and the current IMU bias.
+\end_layout
+
+\begin_layout Standard
+Both variants take a PreintegratedMeasurements object which encodes all
+ the IMU measurements between the previous timestep and the current timestep.
+\end_layout
+
+\begin_layout Standard
+There are also 2 variants of this class:
+\end_layout
+
+\begin_layout Enumerate
+Manifold Preintegration: This version keeps track of the incremental NavState
+ 
+\begin_inset Formula $\Delta X_{ij}$
+\end_inset
+
+ with respect to the previous NavState, on the NavState manifold itself.
+ It also keeps track of the 
+\begin_inset Formula $\Rninesix$
+\end_inset
+
+ Jacobian of 
+\begin_inset Formula $\Delta X_{ij}$
+\end_inset
+
+ w.r.t.
+ the bias.
+ This corresponds to Forster et.
+ al.
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Forster15rss"
+literal "false"
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+Tangent Preintegration: This version keeps track of the incremental NavState
+ in the NavState tangent space instead.
+ This is a 
+\begin_inset Formula $\Rnine$
+\end_inset
+
+ vector 
+\emph on
+preintegrated_
+\emph default
+.
+ It also keeps track of the 
+\begin_inset Formula $\Rninesix$
+\end_inset
+
+ jacobian of the 
+\emph on
+preintegrated_
+\emph default
+ w.r.t.
+ the bias.
+ 
+\end_layout
+
+\begin_layout Standard
+The main function of a factor is to calculate an error.
+ This is done exactly the same in both variants: 
+\begin_inset Formula 
+\begin{equation}
+e(X_{i},X_{j})=X_{j}\ominus\widehat{X_{j}}\label{eq:imu-factor-error}
+\end{equation}
+
+\end_inset
+
+where the predicted NavState 
+\begin_inset Formula $\widehat{X_{j}}$
+\end_inset
+
+ at time 
+\begin_inset Formula $t_{j}$
+\end_inset
+
+ is a function of the NavState 
+\begin_inset Formula $X_{i}$
+\end_inset
+
+ at time 
+\begin_inset Formula $t_{i}$
+\end_inset
+
+ and the preintegrated measurements 
+\begin_inset Formula $PIM$
+\end_inset
+
+:
+\begin_inset Formula 
+\[
+\widehat{X_{j}}=f(X_{i},PIM)
+\]
+
+\end_inset
+
+The noise model associated with this factor is assumed to be zero-mean Gaussian
+ with a 
+\begin_inset Formula $9\times9$
+\end_inset
+
+ covariance matrix 
+\begin_inset Formula $\Sigma_{ij}$
+\end_inset
+
+, which is defined in the tangent space 
+\begin_inset Formula $T_{X_{j}}\mathcal{N}$
+\end_inset
+
+ of the NavState manifold at the NavState 
+\begin_inset Formula $X_{j}$
+\end_inset
+
+.
+ This (discrete-time) covariance matrix is computed in the preintegrated
+ measurement class, of which there are two variants as discussed above.
+\end_layout
+
+\begin_layout Subsubsection*
+Combined IMU Factor
+\end_layout
+
+\begin_layout Standard
+The IMU factor above requires that bias drift over time be modeled as a
+ separate stochastic process (using a BetweenFactor for example), a crucial
+ aspect given that the preintegrated measurements depend on these bias values
+ and are thus correlated.
+ For this reason, we provide another type of IMU factor which we term the
+ Combined IMU Factor.
+ This factor similarly has 2 variants:
+\end_layout
+
+\begin_layout Enumerate
+CombinedImuFactor is a 6-way factor between the previous pose, velocity
+ and IMU bias and the current pose, velocity and IMU bias.
+\end_layout
+
+\begin_layout Enumerate
+CombinedImuFactor2 is a 4-way factor between the previous NavState and IMU
+ bias and the current NavState and IMU bias.
+\end_layout
+
+\begin_layout Standard
+Since the Combined IMU Factor has a larger state variable due to the inclusion
+ of IMU biases, the noise model associated with this factor is assumed to
+ be a zero mean Gaussian with a 
+\begin_inset Formula $15\times15$
+\end_inset
+
+ covariance matrix 
+\begin_inset Formula $\Sigma$
+\end_inset
+
+, similarly defined on the tangent space of the NavState manifold.
+\end_layout
+
+\begin_layout Subsubsection*
+Covariance Matrices
+\end_layout
+
+\begin_layout Standard
+For IMU preintegration, it is important to propagate the uncertainty accurately
+ as well.
+ As such, we detail the various covariance matrices used in the preintegration
+ step.
+\end_layout
+
+\begin_layout Itemize
+Gyroscope Covariance 
+\begin_inset Formula $Q_{\omega}$
+\end_inset
+
+: Measurement uncertainty of the gyroscope.
+\end_layout
+
+\begin_layout Itemize
+Gyroscope Bias Covariance 
+\begin_inset Formula $Q_{\Delta b^{\omega}}$
+\end_inset
+
+ : The covariance associated with the gyroscope bias random walk.
+\end_layout
+
+\begin_layout Itemize
+Accelerometer Covariance 
+\begin_inset Formula $Q_{acc}$
+\end_inset
+
+ : Measurement uncertainty of the accelerometer.
+\end_layout
+
+\begin_layout Itemize
+Accelerometer Bias Covariance 
+\begin_inset Formula $Q_{\Delta b^{acc}}$
+\end_inset
+
+ : The covariance associated with the accelerometer bias random walk.
+\end_layout
+
+\begin_layout Itemize
+Integration Covariance 
+\begin_inset Formula $Q_{int}$
+\end_inset
+
+ : This is the uncertainty due to modeling errors in the integration from
+ acceleration to velocity and position.
+\end_layout
+
+\begin_layout Itemize
+Initial Bias Estimate Covariance 
+\begin_inset Formula $Q_{init}$
+\end_inset
+
+ : This is the uncertainty associated with the estimation of the bias (since
+ we jointly estimate the bias as well).
+\end_layout
+
 \begin_layout Subsubsection*
 Navigation States
 \end_layout
@@ -244,7 +537,7 @@ X(t)=\left\{ R_{0},P_{0}+V_{0}t,V_{0}\right\}
 then the differential equation describing the trajectory is
 \begin_inset Formula 
 \[
-\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\, X(0)=\left\{ R_{0},P_{0},V_{0}\right\} 
+\dot{X}(t)=\left[0_{3x3},V_{0},0_{3x1}\right],\,\,\,\,\,X(0)=\left\{ R_{0},P_{0},V_{0}\right\} 
 \]
 
 \end_inset
@@ -285,7 +578,15 @@ acceleration
 \end_inset
 
  in the body frame.
- We know (from Murray84book) that the derivative of 
+ We know (from 
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Murray94book"
+literal "false"
+
+\end_inset
+
+) that the derivative of 
 \begin_inset Formula $R$
 \end_inset
 
@@ -592,6 +893,7 @@ Lie Group Methods
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Iserles00an"
+literal "true"
 
 \end_inset
 
@@ -602,7 +904,7 @@ key "Iserles00an"
 ,
 \begin_inset Formula 
 \begin{equation}
-\dot{R}(t)=F(R,t),\,\,\,\, R(0)=R_{0}\label{eq:diffSo3}
+\dot{R}(t)=F(R,t),\,\,\,\,R(0)=R_{0}\label{eq:diffSo3}
 \end{equation}
 
 \end_inset
@@ -707,15 +1009,6 @@ In other words, the vector field
 Retractions
 \end_layout
 
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}}
-{\mathfrak{\mathbb{R}^{9}}}
-\end_inset
-
-
-\end_layout
-
 \begin_layout Standard
 Note that the use of the exponential map in local coordinate mappings is
  not obligatory, even in the context of Lie groups.
@@ -947,8 +1240,8 @@ Or, as another way to state this, if we solve the differential equations
 \begin_inset Formula 
 \begin{eqnarray*}
 \dot{\theta}(t) & = & H(\theta)^{-1}\,\omega^{b}(t)\\
-\dot{p}(t) & = & R_{0}^{T}\, V_{0}+v(t)\\
-\dot{v}(t) & = & R_{0}^{T}\, g+R_{b}^{0}(t)a^{b}(t)
+\dot{p}(t) & = & R_{0}^{T}\,V_{0}+v(t)\\
+\dot{v}(t) & = & R_{0}^{T}\,g+R_{b}^{0}(t)a^{b}(t)
 \end{eqnarray*}
 
 \end_inset
@@ -1000,7 +1293,15 @@ In the IMU factor, we need to predict the NavState
 
  needs to be known in order to compensate properly for the initial velocity
  and rotated gravity vector.
- Hence, the idea of Lupton was to split up 
+ Hence, the idea of Lupton
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Lupton12tro"
+literal "false"
+
+\end_inset
+
+ was to split up 
 \begin_inset Formula $v(t)$
 \end_inset
 
@@ -1015,7 +1316,7 @@ v(t)=v_{g}(t)+v_{a}(t)
 evolving as
 \begin_inset Formula 
 \begin{eqnarray*}
-\dot{v}_{g}(t) & = & R_{i}^{T}\, g\\
+\dot{v}_{g}(t) & = & R_{i}^{T}\,g\\
 \dot{v}_{a}(t) & = & R_{b}^{i}(t)a^{b}(t)
 \end{eqnarray*}
 
@@ -1041,7 +1342,7 @@ p(t)=p_{i}(t)+p_{g}(t)+p_{v}(t)
 evolving as
 \begin_inset Formula 
 \begin{eqnarray*}
-\dot{p}_{i}(t) & = & R_{i}^{T}\, V_{i}\\
+\dot{p}_{i}(t) & = & R_{i}^{T}\,V_{i}\\
 \dot{p}_{g}(t) & = & v_{g}(t)=R_{i}^{T}gt\\
 \dot{p}_{v}(t) & = & v_{a}(t)
 \end{eqnarray*}
@@ -1057,8 +1358,11 @@ p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2}
 
 \end_inset
 
-The recipe for the IMU factor is then, in summary.
- Solve the ordinary differential equations
+The recipe for the IMU factor is then, in summary:
+\end_layout
+
+\begin_layout Enumerate
+Solve the ordinary differential equations
 \begin_inset Formula 
 \begin{eqnarray*}
 \dot{\theta}(t) & = & H(\theta(t))^{-1}\,\omega^{b}(t)\\
@@ -1077,7 +1381,10 @@ starting from zero, up to time
 \end_inset
 
  at all times.
- Form the local coordinate vector as
+\end_layout
+
+\begin_layout Enumerate
+Form the local coordinate vector as
 \begin_inset Formula 
 \[
 \zeta(t_{ij})=\left[\theta(t_{ij}),p(t_{ij}),v(t_{ij})\right]=\left[\theta(t_{ij}),R_{i}^{T}V_{i}t_{ij}+R_{i}^{T}\frac{gt_{ij}^{2}}{2}+p_{v}(t_{ij}),R_{i}^{T}gt_{ij}+v_{a}(t_{ij})\right]
@@ -1085,6 +1392,10 @@ starting from zero, up to time
 
 \end_inset
 
+
+\end_layout
+
+\begin_layout Enumerate
 Predict the NavState 
 \begin_inset Formula $X_{j}$
 \end_inset
@@ -1096,7 +1407,7 @@ Predict the NavState
  from
 \begin_inset Formula 
 \[
-X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\, p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\, v_{a}(t_{ij})\right\} 
+X_{j}=\mathcal{R}_{X_{i}}(\zeta(t_{ij}))=\left\{ \Phi_{R_{0}}\left(\theta(t_{ij})\right),P_{i}+V_{i}t_{ij}+\frac{gt_{ij}^{2}}{2}+R_{i}\,p_{v}(t_{ij}),V_{i}+gt_{ij}+R_{i}\,v_{a}(t_{ij})\right\} 
 \]
 
 \end_inset
@@ -1179,11 +1490,59 @@ where we defined the rotation matrix
 \end_layout
 
 \begin_layout Subsubsection*
-Noise Propagation
+Noise Modeling
 \end_layout
 
 \begin_layout Standard
-Even when we assume uncorrelated noise on 
+Given the above solutions to the differential equations, we add noise modeling
+ to account for the various sources of error in the system 
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{eqnarray}
+\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,(\omega_{k}^{b}+\epsilon_{k}^{\omega}-b_{k}^{\omega}-\epsilon_{init}^{\omega})\Delta_{t}\nonumber \\
+p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\frac{\Delta_{t}^{2}}{2}+\epsilon_{k}^{int}\label{eq:preintegration}\\
+v_{k+1} & = & v_{k}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\Delta_{t}\nonumber \\
+b_{k+1}^{a} & = & b_{k}^{a}+\epsilon_{k}^{b^{a}}\nonumber \\
+b_{k+1}^{\omega} & = & b_{k}^{\omega}+\epsilon_{k}^{b^{\omega}}\nonumber 
+\end{eqnarray}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+which we can write compactly as,
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{eqnarray}
+\theta_{k+1} & = & f_{\theta}(\theta_{k},b_{k}^{w},\epsilon_{k}^{\omega},\epsilon_{init}^{b^{\omega}})\label{eq:compact-preintegration}\\
+p_{k+1} & = & f_{p}(p_{k},v_{k},\theta_{k},b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a},\epsilon_{k}^{int})\nonumber \\
+v_{k+1} & = & f_{v}(v_{k,}\theta_{k,}b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a})\nonumber \\
+b_{k+1}^{a} & = & f_{b^{a}}(b_{k}^{a},\epsilon_{k}^{b^{a}})\nonumber \\
+b_{k+1}^{\omega} & = & f_{b^{\omega}}(b_{k}^{\omega},\epsilon_{k}^{b^{\omega}})\nonumber 
+\end{eqnarray}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsubsection*
+Noise Propagation in IMU Factor
+\end_layout
+
+\begin_layout Standard
+We wish to compute the ImuFactor covariance matrix 
+\begin_inset Formula $\Sigma_{ij}$
+\end_inset
+
+.
+ Even when we assume uncorrelated noise on 
 \begin_inset Formula $\omega^{b}$
 \end_inset
 
@@ -1201,11 +1560,12 @@ Even when we assume uncorrelated noise on
 \end_inset
 
  appear in multiple places.
- To model the noise propagation, let us define 
+ To model the noise propagation, let us define the preintegrated navigation
+ state 
 \begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k}]$
 \end_inset
 
- and rewrite Eqns.
+, as a 9D vector on tangent space at and rewrite Eqns.
  (
 \begin_inset CommandInset ref
 LatexCommand ref
@@ -1239,7 +1599,7 @@ Then the noise on
  propagates as
 \begin_inset Formula 
 \begin{equation}
-\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop}
+\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}^{T}+C_{k}\Sigma_{\eta}^{gd}C_{k}^{T}\label{eq:prop}
 \end{equation}
 
 \end_inset
@@ -1280,6 +1640,42 @@ where
 \begin_inset Formula $\omega^{b}$
 \end_inset
 
+.
+ Note that 
+\begin_inset Formula $\Sigma_{k},$
+\end_inset
+
+
+\begin_inset Formula $\Sigma_{\eta}^{ad}$
+\end_inset
+
+, and 
+\begin_inset Formula $\Sigma_{\eta}^{gd}$
+\end_inset
+
+ are discrete time covariances with 
+\begin_inset Formula $\Sigma_{\eta}^{ad}$
+\end_inset
+
+, and 
+\begin_inset Formula $\Sigma_{\eta}^{gd}$
+\end_inset
+
+divided by 
+\begin_inset Formula $\Delta_{t}$
+\end_inset
+
+.
+ Please see the section on Covariance Discretization
+\begin_inset CommandInset ref
+LatexCommand vpageref
+reference "subsec:Covariance-Discretization"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
 .
 \end_layout
 
@@ -1295,7 +1691,7 @@ We start with the noise propagation on
 \begin_layout Standard
 \begin_inset Formula 
 \[
-\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
+\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t}
 \]
 
 \end_inset
@@ -1307,7 +1703,7 @@ It can be shown that for small
  we have 
 \begin_inset Formula 
 \[
-\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}}
+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}
 \]
 
 \end_inset
@@ -1357,9 +1753,9 @@ Putting all this together, we finally obtain
 \begin_inset Formula 
 \[
 A_{k}\approx\left[\begin{array}{ccc}
-I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\
+I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3}\\
 R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
-R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} &  & I_{3\times3}
+R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3}
 \end{array}\right]
 \]
 
@@ -1372,7 +1768,7 @@ B_{k}=\left[\begin{array}{c}
 0_{3\times3}\\
 R_{k}\frac{\Delta_{t}}{2}^{2}\\
 R_{k}\Delta_{t}
-\end{array}\right],\,\,\,\, C_{k}=\left[\begin{array}{c}
+\end{array}\right],\,\,\,\,C_{k}=\left[\begin{array}{c}
 H(\theta_{k})^{-1}\Delta_{t}\\
 0_{3\times3}\\
 0_{3\times3}
@@ -1384,9 +1780,393 @@ H(\theta_{k})^{-1}\Delta_{t}\\
 
 \end_layout
 
+\begin_layout Subsubsection*
+Noise Propagation in Combined IMU Factor
+\end_layout
+
+\begin_layout Standard
+We can similarly account for bias drift over time, as is commonly seen in
+ commercial grade IMUs.
+\end_layout
+
+\begin_layout Standard
+We expand the state vector as 
+\begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$
+\end_inset
+
+ to include the bias terms and define the augmented noise vector 
+\begin_inset Formula $\epsilon=[\epsilon_{k}^{\omega},\epsilon_{k}^{a},\epsilon_{k}^{b^{a}},\epsilon_{k}^{b^{\omega}},\epsilon_{k}^{int},\epsilon_{init}^{b^{a}},\epsilon_{init}^{b^{\omega}}]$
+\end_inset
+
+.
+ This gives the noise propagation equation as
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{equation}
+\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}Q_{k}G_{k}^{T}\label{eq:prop-combined}
+\end{equation}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+where 
+\begin_inset Formula $F_{k}$
+\end_inset
+
+ is the 
+\begin_inset Formula $15\times15$
+\end_inset
+
+ derivative of 
+\begin_inset Formula $f$
+\end_inset
+
+ wrpt this new 
+\begin_inset Formula $\zeta$
+\end_inset
+
+, and 
+\begin_inset Formula $G_{k}$
+\end_inset
+
+ is the 
+\begin_inset Formula $15\times21$
+\end_inset
+
+ matrix for first order uncertainty propagation.
+ 
+\begin_inset Formula $Q_{k}$
+\end_inset
+
+ defines the uncertainty of 
+\begin_inset Formula $\eta$
+\end_inset
+
+.
+ The top-left 
+\begin_inset Formula $9\times9$
+\end_inset
+
+ of 
+\begin_inset Formula $F_{k}$
+\end_inset
+
+ is the same as 
+\begin_inset Formula $A_{k}$
+\end_inset
+
+, thus we only have the jacobians wrpt the biases left to account for.
+\end_layout
+
+\begin_layout Standard
+Conveniently, the jacobians of the pose and velocity wrpt the biases are
+ already computed in the 
+\emph on
+ImuFactor 
+\emph default
+derivation as matrices 
+\begin_inset Formula $B_{k}$
+\end_inset
+
+ and 
+\begin_inset Formula $C_{k}$
+\end_inset
+
+, while they are identity matrices wrpt the biases themselves.
+ Thus, we can easily plug-in the values from the previous section to give
+ us the final result
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+F_{k}\approx\left[\begin{array}{ccccc}
+I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & H(\theta_{k})^{-1}\Delta_{t}\\
+R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2} & 0_{3\times3}\\
+R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\
+0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3}\\
+0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3}
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+Similarly for 
+\begin_inset Formula $Q_{k},$
+\end_inset
+
+we get
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+Q_{k}=\left[\begin{array}{ccccccc}
+\Sigma^{\omega}\\
+ & \Sigma^{a}\\
+ &  & \Sigma^{b^{a}}\\
+ &  &  & \Sigma^{b^{\omega}}\\
+ &  &  &  & \Sigma^{int}\\
+ &  &  &  &  & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
+ &  &  &  &  & \Sigma^{init_{21}} & \Sigma^{init_{22}}
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+and for 
+\begin_inset Formula $G_{k}$
+\end_inset
+
+ we get
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+G_{k}=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}} & \deriv{\theta}{\epsilon^{a}} & \deriv{\theta}{\epsilon^{b^{a}}} & \deriv{\theta}{\epsilon^{b^{\omega}}} & \deriv{\theta}{\epsilon^{int}} & \deriv{\theta}{\epsilon_{init}^{b^{a}}} & \deriv{\theta}{\epsilon_{init}^{b^{\omega}}}\\
+\deriv p{\epsilon^{\omega}} & \deriv p{\epsilon^{a}} & \deriv p{\epsilon^{b^{a}}} & \deriv p{\epsilon^{b^{\omega}}} & \deriv p{\epsilon^{int}} & \deriv p{\epsilon_{init}^{b^{a}}} & \deriv p{\epsilon_{init}^{b^{\omega}}}\\
+\deriv v{\epsilon^{\omega}} & \deriv v{\epsilon^{a}} & \deriv v{\epsilon^{b^{a}}} & \deriv v{\epsilon^{b^{\omega}}} & \deriv v{\epsilon^{int}} & \deriv v{\epsilon_{init}^{b^{a}}} & \deriv v{\epsilon_{init}^{b^{\omega}}}\\
+\deriv{b^{a}}{\epsilon^{\omega}} & \deriv{b^{a}}{\epsilon^{a}} & \deriv{b^{a}}{\epsilon^{b^{a}}} & \deriv{b^{a}}{\epsilon^{b^{\omega}}} & \deriv{b^{a}}{\epsilon^{int}} & \deriv{b^{a}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{a}}{\epsilon_{init}^{b^{\omega}}}\\
+\deriv{b^{\omega}}{\epsilon^{\omega}} & \deriv{b^{\omega}}{\epsilon^{a}} & \deriv{b^{\omega}}{\epsilon^{b^{a}}} & \deriv{b^{\omega}}{\epsilon^{b^{\omega}}} & \deriv{b^{\omega}}{\epsilon^{int}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{\omega}}}
+\end{array}\right]=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
+0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
+0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
+0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+We can perform the block-wise computation of 
+\begin_inset Formula $G_{k}Q_{k}G_{k}^{T}$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\
+0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\
+0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\
+0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0
+\end{array}\right]\left[\begin{array}{ccccccc}
+\Sigma^{\omega}\\
+ & \Sigma^{a}\\
+ &  & \Sigma^{b^{a}}\\
+ &  &  & \Sigma^{b^{\omega}}\\
+ &  &  &  & \Sigma^{int}\\
+ &  &  &  &  & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\
+ &  &  &  &  & \Sigma^{init_{21}} & \Sigma^{init_{22}}
+\end{array}\right]G_{k}^{T}
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
+0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
+0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
+0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
+\end{array}\right]G_{k}^{T}
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{multline*}
+G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc}
+\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\
+0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
+0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\
+0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0
+\end{array}\right]\\
+\left[\begin{array}{ccccc}
+\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
+0 & \deriv p{\epsilon^{a}}^{T} & \deriv v{\epsilon^{a}}^{T} & 0 & 0\\
+0 & 0 & 0 & I_{3\times3} & 0\\
+0 & 0 & 0 & 0 & I_{3\times3}\\
+0 & \deriv p{\epsilon^{int}}^{T} & 0 & 0 & 0\\
+0 & \deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
+\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0
+\end{array}\right]
+\end{multline*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{multline*}
+=\\
+\left[\begin{array}{ccccc}
+\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}\\
+ & +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
+0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
+0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
+\end{array}\right]
+\end{multline*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+which we can break into 3 matrices for clarity, representing the main diagonal
+ and off-diagonal elements
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\begin{multline*}
+=\\
+\left[\begin{array}{ccccc}
+\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\
+0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T} & 0 & 0 & 0\\
+0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T} & 0 & 0\\
+0 & 0 & 0 & \Sigma^{b^{a}} & 0\\
+0 & 0 & 0 & 0 & \Sigma^{b^{\omega}}
+\end{array}\right]+\\
+\left[\begin{array}{ccccc}
+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0\\
+0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
+0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
+0 & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & 0 & 0
+\end{array}\right]+\\
+\left[\begin{array}{ccccc}
+0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\
+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\
+0 & 0 & 0 & 0 & 0\\
+0 & 0 & 0 & 0 & 0
+\end{array}\right]
+\end{multline*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsubsection*
+Covariance Discretization
+\begin_inset CommandInset label
+LatexCommand label
+name "subsec:Covariance-Discretization"
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+So far, all the covariances are assumed to be continuous since the state
+ and measurement models are considered to be continuous-time stochastic
+ processes.
+ However, we sample measurements in a discrete-time fashion, necessitating
+ the need to convert the covariances to their discrete time equivalents.
+\end_layout
+
+\begin_layout Standard
+The IMU is modeled as a first order Gauss-Markov process, with a measurement
+ noise and a process noise.
+ Following 
+\begin_inset CommandInset citation
+LatexCommand cite
+after "Alg. 1 Page 57"
+key "Nikolic16thesis"
+literal "false"
+
+\end_inset
+
+ and 
+\begin_inset CommandInset citation
+LatexCommand cite
+after "Eqns 129-130"
+key "Trawny05report_IndirectKF"
+literal "false"
+
+\end_inset
+
+, the measurement noises 
+\begin_inset Formula $[\epsilon^{a},\epsilon^{\omega},\epsilon_{init}]$
+\end_inset
+
+ are simply scaled by 
+\begin_inset Formula $\frac{1}{\Delta t}$
+\end_inset
+
+, and the process noises 
+\begin_inset Formula $[\epsilon^{int},\epsilon^{b^{a}},\epsilon^{b^{\omega}}]$
+\end_inset
+
+ are scaled by 
+\begin_inset Formula $\Delta t$
+\end_inset
+
+ where 
+\begin_inset Formula $\Delta t$
+\end_inset
+
+ is the time interval between 2 consecutive samples.
+ For a thorough explanation of the discretization process, please refer
+ to 
+\begin_inset CommandInset citation
+LatexCommand cite
+after "Section 8.1"
+key "Simon06book"
+literal "false"
+
+\end_inset
+
+.
+\end_layout
+
 \begin_layout Standard
 \begin_inset CommandInset bibtex
 LatexCommand bibtex
+btprint "btPrintCited"
 bibfiles "refs"
 options "plain"
 
diff --git a/doc/ImuFactor.pdf b/doc/ImuFactor.pdf
index 0b13c1f59..1823cbc4a 100644
Binary files a/doc/ImuFactor.pdf and b/doc/ImuFactor.pdf differ
diff --git a/doc/PreintegratedIMUJacobians.pdf b/doc/PreintegratedIMUJacobians.pdf
new file mode 100644
index 000000000..02616ee07
Binary files /dev/null and b/doc/PreintegratedIMUJacobians.pdf differ
diff --git a/doc/refs.bib b/doc/refs.bib
index 414773483..ec42fb032 100644
--- a/doc/refs.bib
+++ b/doc/refs.bib
@@ -1,26 +1,72 @@
+%% This BibTeX bibliography file was created using BibDesk.
+%% https://bibdesk.sourceforge.io/
+
+%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400 
+
+
+%% Saved with string encoding Unicode (UTF-8) 
+
+
+
+@article{Lupton12tro,
+	author = {Lupton, Todd and Sukkarieh, Salah},
+	date-added = {2021-09-27 17:38:56 -0400},
+	date-modified = {2021-09-27 17:39:09 -0400},
+	doi = {10.1109/TRO.2011.2170332},
+	journal = {IEEE Transactions on Robotics},
+	number = {1},
+	pages = {61-76},
+	title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
+	volume = {28},
+	year = {2012},
+	Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
+
+@inproceedings{Forster15rss,
+	author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
+	booktitle = {Robotics: Science and Systems},
+	date-added = {2021-09-26 20:44:41 -0400},
+	date-modified = {2021-09-26 20:45:03 -0400},
+	title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
+	year = {2015}}
+
 @article{Iserles00an,
-  title =	 {Lie-group methods},
-  author =	 {Iserles, Arieh and Munthe-Kaas, Hans Z and
-                  N{\o}rsett, Syvert P and Zanna, Antonella},
-  journal =	 {Acta Numerica 2000},
-  volume =	 {9},
-  pages =	 {215--365},
-  year =	 {2000},
-  publisher =	 {Cambridge Univ Press}
-}
+	author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
+	journal = {Acta Numerica 2000},
+	pages = {215--365},
+	publisher = {Cambridge Univ Press},
+	title = {Lie-group methods},
+	volume = {9},
+	year = {2000}}
 
 @book{Murray94book,
-  title =	 {A mathematical introduction to robotic manipulation},
-  author =	 {Murray, Richard M and Li, Zexiang and Sastry, S
-                  Shankar and Sastry, S Shankara},
-  year =	 {1994},
-  publisher =	 {CRC press}
-}
+	author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
+	publisher = {CRC press},
+	title = {A mathematical introduction to robotic manipulation},
+	year = {1994}}
 
 @book{Spivak65book,
-  title =	 {Calculus on manifolds},
-  author =	 {Spivak, Michael},
-  volume =	 {1},
-  year =	 {1965},
-  publisher =	 {WA Benjamin New York}
-}
\ No newline at end of file
+	author = {Spivak, Michael},
+	publisher = {WA Benjamin New York},
+	title = {Calculus on manifolds},
+	volume = {1},
+	year = {1965}}
+
+@phdthesis{Nikolic16thesis,
+  title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation},
+  author={Nikolic, Janosch},
+  year={2016},
+  school={ETH Zurich}
+}
+
+@book{Simon06book,
+  title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches},
+  author={Simon, Dan},
+  year={2006},
+  publisher={John Wiley \& Sons}
+}
+
+@inproceedings{Trawny05report_IndirectKF,
+  title={Indirect Kalman Filter for 3 D Attitude Estimation},
+  author={Nikolas Trawny and Stergios I. Roumeliotis},
+  year={2005}
+}
diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp
index 9211a4d5f..e0396ee81 100644
--- a/examples/CombinedImuFactorsExample.cpp
+++ b/examples/CombinedImuFactorsExample.cpp
@@ -60,13 +60,14 @@ namespace po = boost::program_options;
 
 po::variables_map parseOptions(int argc, char* argv[]) {
   po::options_description desc;
-  desc.add_options()("help,h", "produce help message")(
-      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
-      "path to the CSV file with the IMU data")(
-      "output_filename",
-      po::value<string>()->default_value("imuFactorExampleResults.csv"),
-      "path to the result file to use")("use_isam", po::bool_switch(),
-                                        "use ISAM as the optimizer");
+  desc.add_options()("help,h", "produce help message")  // help message
+      ("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
+       "path to the CSV file with the IMU data")  // path to the data file
+      ("output_filename",
+       po::value<string>()->default_value("imuFactorExampleResults.csv"),
+       "path to the result file to use")  // filename to save results to
+      ("use_isam", po::bool_switch(),
+       "use ISAM as the optimizer");  // flag for ISAM optimizer
 
   po::variables_map vm;
   po::store(po::parse_command_line(argc, argv, desc), vm);
@@ -106,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
       I_3x3 * 1e-8;  // error committed in integrating position from velocities
   Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
   Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
+  Matrix66 bias_acc_omega_init =
       I_6x6 * 1e-5;  // error in the bias used for preintegration
 
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@@ -122,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
+  p->biasAccOmegaInt = bias_acc_omega_init;
 
   return p;
 }
diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp
index 38ee4c7c7..c17631864 100644
--- a/examples/ImuFactorsExample.cpp
+++ b/examples/ImuFactorsExample.cpp
@@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
       I_3x3 * 1e-8;  // error committed in integrating position from velocities
   Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
   Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
+  Matrix66 bias_acc_omega_init =
       I_6x6 * 1e-5;  // error in the bias used for preintegration
 
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
+  p->biasAccOmegaInt = bias_acc_omega_init;
 
   return p;
 }
diff --git a/examples/README.md b/examples/README.md
index 5a72736e0..924a79521 100644
--- a/examples/README.md
+++ b/examples/README.md
@@ -33,7 +33,6 @@ The following examples illustrate some concepts from Georgia Tech's research pap
 ## 2D Pose SLAM 
 
 * **LocalizationExample.cpp**: modeling robot motion
-* **LocalizationExample2.cpp**: example with GPS like measurements
 * **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
 * **Pose2SLAMExample_advanced**: same, but uses an Optimizer object
 * **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h
index 39541416e..89fa8248c 100644
--- a/gtsam/base/Group.h
+++ b/gtsam/base/Group.h
@@ -95,7 +95,7 @@ template<class Class>
 struct MultiplicativeGroupTraits {
   typedef group_tag structure_category;
   typedef multiplicative_group_tag group_flavor;
-  static Class Identity() { return Class::identity(); }
+  static Class Identity() { return Class::Identity(); }
   static Class Compose(const Class &g, const Class & h) { return g * h;}
   static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
   static Class Inverse(const Class &g) { return g.inverse();}
@@ -111,7 +111,7 @@ template<class Class>
 struct AdditiveGroupTraits {
   typedef group_tag structure_category;
   typedef additive_group_tag group_flavor;
-  static Class Identity() { return Class::identity(); }
+  static Class Identity() { return Class::Identity(); }
   static Class Compose(const Class &g, const Class & h) { return g + h;}
   static Class Between(const Class &g, const Class & h) { return h - g;}
   static Class Inverse(const Class &g) { return -g;}
@@ -147,7 +147,7 @@ public:
   DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
 
   // identity
-  static DirectProduct identity() { return DirectProduct(); }
+  static DirectProduct Identity() { return DirectProduct(); }
 
   DirectProduct operator*(const DirectProduct& other) const {
     return DirectProduct(traits<G>::Compose(this->first, other.first),
@@ -181,7 +181,7 @@ public:
   DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
 
   // identity
-  static DirectSum identity() { return DirectSum(); }
+  static DirectSum Identity() { return DirectSum(); }
 
   DirectSum operator+(const DirectSum& other) const {
     return DirectSum(g()+other.g(), h()+other.h());
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index cb8e7d017..c4eba5deb 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -177,7 +177,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
   /// @name Group
   /// @{
   typedef multiplicative_group_tag group_flavor;
-  static Class Identity() { return Class::identity();}
+  static Class Identity() { return Class::Identity();}
   /// @}
 
   /// @name Manifold
diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h
index a2dcf738e..6689c11d6 100644
--- a/gtsam/base/ProductLieGroup.h
+++ b/gtsam/base/ProductLieGroup.h
@@ -48,7 +48,7 @@ public:
   /// @name Group
   /// @{
   typedef multiplicative_group_tag group_flavor;
-  static ProductLieGroup identity() {return ProductLieGroup();}
+  static ProductLieGroup Identity() {return ProductLieGroup();}
 
   ProductLieGroup operator*(const ProductLieGroup& other) const {
     return ProductLieGroup(traits<G>::Compose(this->first,other.first),
diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index 9cb2aa165..f7923ff88 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -60,6 +60,7 @@ GTSAM_MAKE_VECTOR_DEFS(9)
 GTSAM_MAKE_VECTOR_DEFS(10)
 GTSAM_MAKE_VECTOR_DEFS(11)
 GTSAM_MAKE_VECTOR_DEFS(12)
+GTSAM_MAKE_VECTOR_DEFS(15)
 
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h
index 43644b5c4..f7809f596 100644
--- a/gtsam/base/VectorSpace.h
+++ b/gtsam/base/VectorSpace.h
@@ -169,7 +169,7 @@ struct HasVectorSpacePrereqs {
   Vector v;
 
   BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
-    p = Class::identity();  // identity
+    p = Class::Identity();  // identity
     q = p + p;              // addition
     q = p - p;              // subtraction
     v = p.vector();         // conversion to vector
@@ -192,7 +192,7 @@ struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
   /// @name Group
   /// @{
   typedef additive_group_tag group_flavor;
-  static Class Identity() { return Class::identity();}
+  static Class Identity() { return Class::Identity();}
   /// @}
 
   /// @name Manifold
diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp
index 976dee697..7e8cb7821 100644
--- a/gtsam/base/tests/testGroup.cpp
+++ b/gtsam/base/tests/testGroup.cpp
@@ -29,7 +29,7 @@ class Symmetric: private Eigen::PermutationMatrix<N> {
       Eigen::PermutationMatrix<N>(P) {
   }
 public:
-  static Symmetric identity() { return Symmetric(); }
+  static Symmetric Identity() { return Symmetric(); }
   Symmetric() {
     Eigen::PermutationMatrix<N>::setIdentity();
   }
diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h
index eddcbfeae..3f830ec51 100644
--- a/gtsam/basis/ParameterMatrix.h
+++ b/gtsam/basis/ParameterMatrix.h
@@ -189,9 +189,9 @@ class ParameterMatrix {
    * NOTE: The size at compile time is unknown so this identity is zero
    * length and thus not valid.
    */
-  inline static ParameterMatrix identity() {
+  inline static ParameterMatrix Identity() {
     // throw std::runtime_error(
-    //     "ParameterMatrix::identity(): Don't use this function");
+    //     "ParameterMatrix::Identity(): Don't use this function");
     return ParameterMatrix(0);
   }
 
diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp
index ec62e8eea..ae2e8e111 100644
--- a/gtsam/basis/tests/testParameterMatrix.cpp
+++ b/gtsam/basis/tests/testParameterMatrix.cpp
@@ -133,7 +133,7 @@ TEST(ParameterMatrix, VectorSpace) {
   // vector
   EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
   // identity
-  EXPECT(assert_equal(ParameterMatrix<M>::identity(),
+  EXPECT(assert_equal(ParameterMatrix<M>::Identity(),
                       ParameterMatrix<M>(Matrix::Zero(M, 0))));
 }
 
diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h
index 065cd0140..a503a6072 100644
--- a/gtsam/geometry/Cyclic.h
+++ b/gtsam/geometry/Cyclic.h
@@ -38,7 +38,7 @@ public:
   /// Default constructor yields identity
   Cyclic():i_(0) {
   }
-  static Cyclic identity() { return Cyclic();}
+  static Cyclic Identity() { return Cyclic();}
 
   /// Cast to size_t
   operator size_t() const {
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index c20e90819..e5a8eec7a 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -213,7 +213,7 @@ public:
   }
 
   /// for Canonical
-  static PinholeCamera identity() {
+  static PinholeCamera Identity() {
     return PinholeCamera(); // assumes that the default constructor is valid
   }
 
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index 7b92be5d5..ce393e45f 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -412,7 +412,7 @@ public:
   }
 
   /// for Canonical
-  static PinholePose identity() {
+  static PinholePose Identity() {
     return PinholePose(); // assumes that the default constructor is valid
   }
 
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 466c5a42a..008e6525d 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -119,7 +119,7 @@ public:
   /// @{
 
   /// identity for group operation
-  inline static Pose2 identity() { return Pose2(); }
+  inline static Pose2 Identity() { return Pose2(); }
 
   /// inverse
   GTSAM_EXPORT Pose2 inverse() const;
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 33fb55250..6dec7f74f 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -103,7 +103,7 @@ public:
   /// @{
 
   /// identity for group operation
-  static Pose3 identity() {
+  static Pose3 Identity() {
     return Pose3();
   }
 
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 2690ca248..be8d11cda 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -107,8 +107,8 @@ namespace gtsam {
     /// @name Group
     /// @{
 
-    /** identity */
-    inline static Rot2 identity() {  return Rot2(); }
+    /** Identity */
+    inline static Rot2 Identity() {  return Rot2(); }
 
     /** The inverse rotation - negative angle */
     Rot2 inverse() const { return Rot2(c_, -s_);}
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 01ca7778c..800ce4cdf 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -297,7 +297,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     /// @{
 
     /// identity rotation for group operation
-    inline static Rot3 identity() {
+    inline static Rot3 Identity() {
       return Rot3();
     }
 
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index af0e7a3cf..7d1e6db7a 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -178,13 +178,13 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
 
   /// SO<N> identity for N >= 2
   template <int N_ = N, typename = IsFixed<N_>>
-  static SO identity() {
+  static SO Identity() {
     return SO();
   }
 
   /// SO<N> identity for N == Eigen::Dynamic
   template <int N_ = N, typename = IsDynamic<N_>>
-  static SO identity(size_t n = 0) {
+  static SO Identity(size_t n = 0) {
     return SO(n);
   }
 
diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index 4ed3351f8..4b8cfd581 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -134,7 +134,7 @@ void Similarity2::print(const std::string& s) const {
             << std::endl;
 }
 
-Similarity2 Similarity2::identity() { return Similarity2(); }
+Similarity2 Similarity2::Identity() { return Similarity2(); }
 
 Similarity2 Similarity2::operator*(const Similarity2& S) const {
   return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index 05f10d149..e72cda484 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -83,7 +83,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// @{
 
   /// Return an identity transform
-  static Similarity2 identity();
+  static Similarity2 Identity();
 
   /// Composition
   Similarity2 operator*(const Similarity2& S) const;
diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
index 7fde974c5..146161796 100644
--- a/gtsam/geometry/Similarity3.cpp
+++ b/gtsam/geometry/Similarity3.cpp
@@ -122,7 +122,7 @@ void Similarity3::print(const std::string& s) const {
   std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
 }
 
-Similarity3 Similarity3::identity() {
+Similarity3 Similarity3::Identity() {
   return Similarity3();
 }
 Similarity3 Similarity3::operator*(const Similarity3& S) const {
diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h
index 845d4c810..76d2bf536 100644
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@@ -84,7 +84,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// @{
 
   /// Return an identity transform
-  static Similarity3 identity();
+  static Similarity3 Identity();
 
   /// Composition
   Similarity3 operator*(const Similarity3& S) const;
diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h
index 4880423d3..adcbe46a5 100644
--- a/gtsam/geometry/SphericalCamera.h
+++ b/gtsam/geometry/SphericalCamera.h
@@ -88,7 +88,7 @@ class GTSAM_EXPORT SphericalCamera {
 
   /// Default constructor
   SphericalCamera()
-      : pose_(Pose3::identity()), emptyCal_(boost::make_shared<EmptyCal>()) {}
+      : pose_(Pose3()), emptyCal_(boost::make_shared<EmptyCal>()) {}
 
   /// Constructor with pose
   explicit SphericalCamera(const Pose3& pose)
@@ -198,9 +198,9 @@ class GTSAM_EXPORT SphericalCamera {
   }
 
   /// for Canonical
-  static SphericalCamera identity() {
+  static SphericalCamera Identity() {
     return SphericalCamera(
-        Pose3::identity());  // assumes that the default constructor is valid
+        Pose3::Identity());  // assumes that the default constructor is valid
   }
 
   /// for Linear Triangulation
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index 9eef01577..8c9197fd2 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -71,7 +71,7 @@ public:
   /// @{
 
   /// identity
-  inline static StereoPoint2 identity() {
+  inline static StereoPoint2 Identity() {
     return StereoPoint2();
   }
 
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 1b1169d2f..988727b98 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -16,7 +16,7 @@ class Point2 {
   bool equals(const gtsam::Point2& point, double tol) const;
 
   // Group
-  static gtsam::Point2 identity();
+  static gtsam::Point2 Identity();
 
   // Standard Interface
   double x() const;
@@ -73,7 +73,7 @@ class StereoPoint2 {
   bool equals(const gtsam::StereoPoint2& point, double tol) const;
 
   // Group
-  static gtsam::StereoPoint2 identity();
+  static gtsam::StereoPoint2 Identity();
   gtsam::StereoPoint2 inverse() const;
   gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
   gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
@@ -115,7 +115,7 @@ class Point3 {
   bool equals(const gtsam::Point3& p, double tol) const;
 
   // Group
-  static gtsam::Point3 identity();
+  static gtsam::Point3 Identity();
 
   // Standard Interface
   Vector vector() const;
@@ -149,7 +149,7 @@ class Rot2 {
   bool equals(const gtsam::Rot2& rot, double tol) const;
 
   // Group
-  static gtsam::Rot2 identity();
+  static gtsam::Rot2 Identity();
   gtsam::Rot2 inverse();
   gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
   gtsam::Rot2 between(const gtsam::Rot2& p2) const;
@@ -198,7 +198,7 @@ class SO3 {
   bool equals(const gtsam::SO3& other, double tol) const;
 
   // Group
-  static gtsam::SO3 identity();
+  static gtsam::SO3 Identity();
   gtsam::SO3 inverse() const;
   gtsam::SO3 between(const gtsam::SO3& R) const;
   gtsam::SO3 compose(const gtsam::SO3& R) const;
@@ -228,7 +228,7 @@ class SO4 {
   bool equals(const gtsam::SO4& other, double tol) const;
 
   // Group
-  static gtsam::SO4 identity();
+  static gtsam::SO4 Identity();
   gtsam::SO4 inverse() const;
   gtsam::SO4 between(const gtsam::SO4& Q) const;
   gtsam::SO4 compose(const gtsam::SO4& Q) const;
@@ -258,7 +258,7 @@ class SOn {
   bool equals(const gtsam::SOn& other, double tol) const;
 
   // Group
-  static gtsam::SOn identity();
+  static gtsam::SOn Identity();
   gtsam::SOn inverse() const;
   gtsam::SOn between(const gtsam::SOn& Q) const;
   gtsam::SOn compose(const gtsam::SOn& Q) const;
@@ -322,7 +322,7 @@ class Rot3 {
   bool equals(const gtsam::Rot3& rot, double tol) const;
 
   // Group
-  static gtsam::Rot3 identity();
+  static gtsam::Rot3 Identity();
   gtsam::Rot3 inverse() const;
   gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
   gtsam::Rot3 between(const gtsam::Rot3& p2) const;
@@ -380,7 +380,7 @@ class Pose2 {
   bool equals(const gtsam::Pose2& pose, double tol) const;
 
   // Group
-  static gtsam::Pose2 identity();
+  static gtsam::Pose2 Identity();
   gtsam::Pose2 inverse() const;
   gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
   gtsam::Pose2 between(const gtsam::Pose2& p2) const;
@@ -444,7 +444,7 @@ class Pose3 {
   bool equals(const gtsam::Pose3& pose, double tol) const;
 
   // Group
-  static gtsam::Pose3 identity();
+  static gtsam::Pose3 Identity();
   gtsam::Pose3 inverse() const;
   gtsam::Pose3 inverse(Eigen::Ref<Eigen::MatrixXd> H) const;
   gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index de779cc75..88c00a2e9 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -902,7 +902,7 @@ TEST(Pose2 , TransformCovariance3) {
 
 /* ************************************************************************* */
 TEST(Pose2, Print) {
-  Pose2 pose(Rot2::identity(), Point2(1, 2));
+  Pose2 pose(Rot2::Identity(), Point2(1, 2));
 
   // Generate the expected output
   string s = "Planar Pose";
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index e1d3d5ea2..78a4af799 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -1133,7 +1133,7 @@ Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { retu
 
 TEST(Pose3, interpolateJacobians) {
   {
-    Pose3 X = Pose3::identity();
+    Pose3 X = Pose3::Identity();
     Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
     double t = 0.5;
     Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
@@ -1147,10 +1147,10 @@ TEST(Pose3, interpolateJacobians) {
     EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
   }
   {
-    Pose3 X = Pose3::identity();
-    Pose3 Y(Rot3::identity(), Point3(1, 0, 0));
+    Pose3 X = Pose3::Identity();
+    Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
     double t = 0.3;
-    Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0));
+    Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0));
     Matrix actualJacobianX, actualJacobianY;
     EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
 
@@ -1161,7 +1161,7 @@ TEST(Pose3, interpolateJacobians) {
     EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
   }
   {
-    Pose3 X = Pose3::identity();
+    Pose3 X = Pose3::Identity();
     Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
     double t = 0.5;
     Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
@@ -1204,7 +1204,7 @@ TEST(Pose3, Create) {
 
 /* ************************************************************************* */
 TEST(Pose3, Print) {
-  Pose3 pose(Rot3::identity(), Point3(1, 2, 3));
+  Pose3 pose(Rot3::Identity(), Point3(1, 2, 3));
 
   // Generate the expected output
   std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";
diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp
index 7a134f6ef..fad24f743 100644
--- a/gtsam/geometry/tests/testSimilarity3.cpp
+++ b/gtsam/geometry/tests/testSimilarity3.cpp
@@ -203,11 +203,11 @@ TEST(Similarity3, ExpLogMap) {
 
   Vector7 zeros;
   zeros << 0, 0, 0, 0, 0, 0, 0;
-  Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity());
+  Vector7 logIdentity = Similarity3::Logmap(Similarity3::Identity());
   EXPECT(assert_equal(zeros, logIdentity));
 
   Similarity3 expZero = Similarity3::Expmap(zeros);
-  Similarity3 ident = Similarity3::identity();
+  Similarity3 ident = Similarity3::Identity();
   EXPECT(assert_equal(expZero, ident));
 
   // Compare to matrix exponential, using expm in Lie.h
@@ -391,7 +391,7 @@ TEST(Similarity3, Optimization) {
   NonlinearFactorGraph graph;
   graph.addPrior(key, prior, model);
 
-  // Create initial estimate with identity transform
+  // Create initial estimate with Identity transform
   Values initial;
   initial.insert<Similarity3>(key, Similarity3());
 
diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp
index 3fe2cf4d1..8d3a7dd31 100644
--- a/gtsam/navigation/CombinedImuFactor.cpp
+++ b/gtsam/navigation/CombinedImuFactor.cpp
@@ -93,9 +93,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
 //------------------------------------------------------------------------------
 void PreintegratedCombinedMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
+  if (dt <= 0) {
+    throw std::runtime_error(
+        "PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
+  }
+
   // Update preintegrated measurements.
-  Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
-  Matrix93 B, C;
+  Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx)
+  Matrix93 B, C;  // Jacobian of state wrpt accel bias and omega bias respectively.
   PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
   // Update preintegrated measurements covariance: as in [2] we consider a first
@@ -105,47 +110,78 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // and preintegrated measurements
 
   // Single Jacobians to propagate covariance
-  // TODO(frank): should we not also account for bias on position?
-  Matrix3 theta_H_biasOmega = -C.topRows<3>();
-  Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
+  Matrix3 theta_H_biasOmega = C.topRows<3>();
+  Matrix3 pos_H_biasAcc = B.middleRows<3>(3);
+  Matrix3 vel_H_biasAcc = B.bottomRows<3>();
+
+  Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega;
+  Matrix3 pos_H_biasAccInit = -pos_H_biasAcc;
+  Matrix3 vel_H_biasAccInit = -vel_H_biasAcc;
 
   // overall Jacobian wrt preintegrated measurements (df/dx)
   Eigen::Matrix<double, 15, 15> F;
   F.setZero();
   F.block<9, 9>(0, 0) = A;
   F.block<3, 3>(0, 12) = theta_H_biasOmega;
+  F.block<3, 3>(3, 9) = pos_H_biasAcc;
   F.block<3, 3>(6, 9) = vel_H_biasAcc;
   F.block<6, 6>(9, 9) = I_6x6;
 
+  // Update the uncertainty on the state (matrix F in [4]).
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose();
+
   // propagate uncertainty
   // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
   const Matrix3& aCov = p().accelerometerCovariance;
   const Matrix3& wCov = p().gyroscopeCovariance;
   const Matrix3& iCov = p().integrationCovariance;
+  const Matrix6& bInitCov = p().biasAccOmegaInt;
 
   // first order uncertainty propagation
-  // Optimized matrix multiplication   (1/dt) * G * measurementCovariance *
-  // G.transpose()
+  // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
   Eigen::Matrix<double, 15, 15> G_measCov_Gt;
   G_measCov_Gt.setZero(15, 15);
 
+  const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt;
+  const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt;
+  const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt;
+  const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt;
+
   // BLOCK DIAGONAL TERMS
-  D_t_t(&G_measCov_Gt) = dt * iCov;
-  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
-      * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
-      * (vel_H_biasAcc.transpose());
-  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
-      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
-      * (theta_H_biasOmega.transpose());
+  D_R_R(&G_measCov_Gt) =
+      (theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose())  //
+      +
+      (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
+
+  D_t_t(&G_measCov_Gt) =
+      (pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose())           //
+      + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose())  //
+      + (dt * iCov);
+
+  D_v_v(&G_measCov_Gt) =
+      (vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose())  //
+      + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
+
   D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
   D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
 
   // OFF BLOCK DIAGONAL TERMS
-  Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
-      * theta_H_biasOmega.transpose();
-  D_v_R(&G_measCov_Gt) = temp;
-  D_R_v(&G_measCov_Gt) = temp.transpose();
-  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
+  D_R_t(&G_measCov_Gt) =
+      theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
+  D_R_v(&G_measCov_Gt) =
+      theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
+  D_t_R(&G_measCov_Gt) =
+      pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
+  D_t_v(&G_measCov_Gt) =
+      (pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) +
+      (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
+  D_v_R(&G_measCov_Gt) =
+      vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
+  D_v_t(&G_measCov_Gt) =
+      (vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) +
+      (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
+
+  preintMeasCov_.noalias() += G_measCov_Gt;
 }
 
 //------------------------------------------------------------------------------
@@ -253,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
   os << "  noise model sigmas: " << f.noiseModel_->sigmas().transpose();
   return os;
 }
-}
- /// namespace gtsam
 
+}  // namespace gtsam
diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 54c5a7dbb..69d72ad9b 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -51,6 +51,7 @@ typedef ManifoldPreintegration PreintegrationType;
  *     TRO, 28(1):61-76, 2012.
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
  *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ *     Available in this repo as "PreintegratedIMUJacobians.pdf".
  * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
  *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
  *     Robotics: Science and Systems (RSS), 2015.
@@ -61,7 +62,7 @@ typedef ManifoldPreintegration PreintegrationType;
 struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
   Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing accelerometer bias random walk
   Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
-  Matrix6 biasAccOmegaInt;     ///< covariance of bias used for pre-integration
+  Matrix6 biasAccOmegaInt;     ///< covariance of bias used as initial estimate.
 
   /// Default constructor makes uninitialized params struct.
   /// Used for serialization.
@@ -92,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
 
   void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
   void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
-  void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
+  void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
   
   const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
   const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
-  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
+  const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
   
 private:
 
diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h
index 9346a4a77..936ae7782 100644
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@@ -105,7 +105,7 @@ public:
   /// @{
 
   /** identity for group operation */
-  static ConstantBias identity() {
+  static ConstantBias Identity() {
     return ConstantBias();
   }
 
diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp
index 28c0461b1..9b6affaaf 100644
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
 
   // Update preintegrated measurements (also get Jacobian)
   Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
-  Matrix93 B, C;
+  Matrix93 B, C;  // Jacobian of state wrpt accel bias and omega bias respectively.
   PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
   // first order covariance propagation:
@@ -73,11 +73,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   const Matrix3& iCov = p().integrationCovariance;
 
   // (1/dt) allows to pass from continuous time noise to discrete time noise
+  // Update the uncertainty on the state (matrix A in [4]).
   preintMeasCov_ = A * preintMeasCov_ * A.transpose();
+  // These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]).
   preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
   preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
 
-  // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
+  // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix)
   preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
 }
 
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index c3b398e22..6765c8b42 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -53,6 +53,7 @@ typedef ManifoldPreintegration PreintegrationType;
  *     TRO, 28(1):61-76, 2012.
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
  *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ *     Available in this repo as "PreintegratedIMUJacobians.pdf".
  * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
  *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
  *     Robotics: Science and Systems (RSS), 2015.
diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index 8840c34e9..f6e9fccb8 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -157,9 +157,9 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i,
       state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
                                H1 || H3 ? &D_error_predict : 0);
 
-  if (H1) *H1 << D_error_predict* D_predict_state_i;
+  if (H1) *H1 << D_error_predict * D_predict_state_i;
   if (H2) *H2 << D_error_state_j;
-  if (H3) *H3 << D_error_predict* D_predict_bias_i;
+  if (H3) *H3 << D_error_predict * D_predict_bias_i;
 
   return error;
 }
diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp
index 3938ce86c..9d3e258de 100644
--- a/gtsam/navigation/ScenarioRunner.cpp
+++ b/gtsam/navigation/ScenarioRunner.cpp
@@ -15,8 +15,8 @@
  * @author  Frank Dellaert
  */
 
-#include <gtsam/navigation/ScenarioRunner.h>
 #include <gtsam/base/timing.h>
+#include <gtsam/navigation/ScenarioRunner.h>
 
 #include <boost/assign.hpp>
 #include <cmath>
@@ -105,4 +105,62 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
   return Q / (N - 1);
 }
 
+PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
+    double T, const Bias& estimatedBias, bool corrupted) const {
+  gttic_(integrate);
+  PreintegratedCombinedMeasurements pim(p_, estimatedBias);
+
+  const double dt = imuSampleTime();
+  const size_t nrSteps = T / dt;
+  double t = 0;
+  for (size_t k = 0; k < nrSteps; k++, t += dt) {
+    Vector3 measuredOmega =
+        corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
+    Vector3 measuredAcc =
+        corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
+    pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
+  }
+
+  return pim;
+}
+
+NavState CombinedScenarioRunner::predict(
+    const PreintegratedCombinedMeasurements& pim,
+    const Bias& estimatedBias) const {
+  const NavState state_i(scenario().pose(0), scenario().velocity_n(0));
+  return pim.predict(state_i, estimatedBias);
+}
+
+Eigen::Matrix<double, 15, 15> CombinedScenarioRunner::estimateCovariance(
+    double T, size_t N, const Bias& estimatedBias) const {
+  gttic_(estimateCovariance);
+
+  // Get predict prediction from ground truth measurements
+  NavState prediction = predict(integrate(T));
+
+  // Sample !
+  Matrix samples(15, N);
+  Vector15 sum = Vector15::Zero();
+  for (size_t i = 0; i < N; i++) {
+    auto pim = integrate(T, estimatedBias, true);
+    NavState sampled = predict(pim);
+    Vector15 xi = Vector15::Zero();
+    xi << sampled.localCoordinates(prediction),
+        (estimatedBias_.vector() - estimatedBias.vector());
+    samples.col(i) = xi;
+    sum += xi;
+  }
+
+  // Compute MC covariance
+  Vector15 sampleMean = sum / N;
+  Eigen::Matrix<double, 15, 15> Q;
+  Q.setZero();
+  for (size_t i = 0; i < N; i++) {
+    Vector15 xi = samples.col(i) - sampleMean;
+    Q += xi * xi.transpose();
+  }
+
+  return Q / (N - 1);
+}
+
 }  // namespace gtsam
diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h
index 1577e36fe..cee5a54ab 100644
--- a/gtsam/navigation/ScenarioRunner.h
+++ b/gtsam/navigation/ScenarioRunner.h
@@ -16,9 +16,10 @@
  */
 
 #pragma once
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/Scenario.h>
-#include <gtsam/linear/Sampler.h>
 
 namespace gtsam {
 
@@ -66,10 +67,10 @@ class GTSAM_EXPORT ScenarioRunner {
   // also, uses g=10 for easy debugging
   const Vector3& gravity_n() const { return p_->n_gravity; }
 
+  const Scenario& scenario() const { return scenario_; }
+
   // A gyro simply measures angular velocity in body frame
-  Vector3 actualAngularVelocity(double t) const {
-    return scenario_.omega_b(t);
-  }
+  Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
 
   // An accelerometer measures acceleration in body, but not gravity
   Vector3 actualSpecificForce(double t) const {
@@ -106,4 +107,39 @@ class GTSAM_EXPORT ScenarioRunner {
   Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
 };
 
+/*
+ *  Simple class to test navigation scenarios with CombinedImuMeasurements.
+ *  Takes a trajectory scenario as input, and can generate IMU measurements
+ */
+class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
+ public:
+  typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
+
+ private:
+  const SharedParams p_;
+  const Bias estimatedBias_;
+
+ public:
+  CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
+                         double imuSampleTime = 1.0 / 100.0,
+                         const Bias& bias = Bias())
+      : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
+                       imuSampleTime, bias),
+        p_(p),
+        estimatedBias_(bias) {}
+
+  /// Integrate measurements for T seconds into a PIM
+  PreintegratedCombinedMeasurements integrate(
+      double T, const Bias& estimatedBias = Bias(),
+      bool corrupted = false) const;
+
+  /// Predict predict given a PIM
+  NavState predict(const PreintegratedCombinedMeasurements& pim,
+                   const Bias& estimatedBias = Bias()) const;
+
+  /// Compute a Monte Carlo estimate of the predict covariance using N samples
+  Eigen::Matrix<double, 15, 15> estimateCovariance(
+      double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
+};
+
 }  // namespace gtsam
diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i
index 6ede1645f..731cf3807 100644
--- a/gtsam/navigation/navigation.i
+++ b/gtsam/navigation/navigation.i
@@ -17,7 +17,7 @@ class ConstantBias {
   bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
 
   // Group
-  static gtsam::imuBias::ConstantBias identity();
+  static gtsam::imuBias::ConstantBias Identity();
 
   // Operator Overloads
   gtsam::imuBias::ConstantBias operator-() const;
@@ -165,11 +165,11 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
 
   void setBiasAccCovariance(Matrix cov);
   void setBiasOmegaCovariance(Matrix cov);
-  void setBiasAccOmegaInt(Matrix cov);
+  void setBiasAccOmegaInit(Matrix cov);
   
   Matrix getBiasAccCovariance() const ;
   Matrix getBiasOmegaCovariance() const ;
-  Matrix getBiasAccOmegaInt() const;
+  Matrix getBiasAccOmegaInit() const;
  
 };
 
diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp
index 2bbc2cc7c..aacfff0f0 100644
--- a/gtsam/navigation/tests/testCombinedImuFactor.cpp
+++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp
@@ -16,18 +16,19 @@
  * @author  Frank Dellaert
  * @author  Richard Roberts
  * @author  Stephen Williams
+ * @author  Varun Agrawal
  */
 
-#include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/navigation/CombinedImuFactor.h>
-#include <gtsam/navigation/ImuBias.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/inference/Symbol.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
-
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/navigation/ScenarioRunner.h>
+#include <gtsam/nonlinear/Values.h>
 
 #include <list>
 
@@ -40,12 +41,15 @@ static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
   p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
   p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
   p->integrationCovariance = 0.0001 * I_3x3;
+  p->biasAccCovariance = Z_3x3;
+  p->biasOmegaCovariance = Z_3x3;
+  p->biasAccOmegaInt = Z_6x6;
   return p;
 }
-}
+}  // namespace testing
 
 /* ************************************************************************* */
-TEST( CombinedImuFactor, PreintegratedMeasurements ) {
+TEST(CombinedImuFactor, PreintegratedMeasurements ) {
   // Linearization point
   Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
 
@@ -71,8 +75,9 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
   DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
 }
 
+
 /* ************************************************************************* */
-TEST( CombinedImuFactor, ErrorWithBiases ) {
+TEST(CombinedImuFactor, ErrorWithBiases ) {
   Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
   Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
@@ -203,6 +208,114 @@ TEST(CombinedImuFactor, PredictRotation) {
   EXPECT(assert_equal(expectedPose, actual.pose(), tol));
 }
 
+/* ************************************************************************* */
+// Testing covariance to check if all the jacobians are accounted for.
+TEST(CombinedImuFactor, CheckCovariance) {
+  auto params = PreintegrationCombinedParams::MakeSharedU(9.81);
+
+  params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
+  params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
+  params->setIntegrationCovariance(pow(0.0, 2) * I_3x3);
+  params->setOmegaCoriolis(Vector3::Zero());
+
+  imuBias::ConstantBias currentBias;
+
+  PreintegratedCombinedMeasurements actual(params, currentBias);
+
+  // Measurements
+  Vector3 measuredAcc(0.1577, -0.8251, 9.6111);
+  Vector3 measuredOmega(-0.0210, 0.0311, 0.0145);
+  double deltaT = 0.01;
+
+  actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+
+  Eigen::Matrix<double, 15, 15> expected;
+  expected << 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,          //
+      0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,                  //
+      0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,                  //
+      0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0, 0,  //
+      0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0, 0,  //
+      0, 0, 0, 0, 0, 2.50025e-07, 0, 0, 5.0005e-05, 0, 0, 0, 0, 0, 0,  //
+      0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0, 0,     //
+      0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0, 0,     //
+      0, 0, 0, 0, 0, 5.0005e-05, 0, 0, 0.010001, 0, 0, 0, 0, 0, 0,     //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,                  //
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
+
+  // regression
+  EXPECT(assert_equal(expected, actual.preintMeasCov()));
+}
+
+// Test that the covariance values for the ImuFactor and the CombinedImuFactor
+// (top-left 9x9) are the same
+TEST(CombinedImuFactor, SameCovariance) {
+  // IMU measurements and time delta
+  Vector3 accMeas(0.1577, -0.8251, 9.6111);
+  Vector3 omegaMeas(-0.0210, 0.0311, 0.0145);
+  double deltaT = 0.01;
+
+  // Assume zero bias
+  imuBias::ConstantBias currentBias;
+
+  // Define params for ImuFactor
+  auto params = PreintegrationParams::MakeSharedU();
+  params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
+  params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
+  params->setIntegrationCovariance(pow(0, 2) * I_3x3);
+  params->setOmegaCoriolis(Vector3::Zero());
+
+  // The IMU preintegration object for ImuFactor
+  PreintegratedImuMeasurements pim(params, currentBias);
+  pim.integrateMeasurement(accMeas, omegaMeas, deltaT);
+
+  // Define params for CombinedImuFactor
+  auto combined_params = PreintegrationCombinedParams::MakeSharedU();
+  combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
+  combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3);
+  // Set bias integration covariance explicitly to zero
+  combined_params->setIntegrationCovariance(Z_3x3);
+  combined_params->setOmegaCoriolis(Z_3x1);
+  // Set bias initial covariance explicitly to zero
+  combined_params->setBiasAccOmegaInit(Z_6x6);
+
+  // The IMU preintegration object for CombinedImuFactor
+  PreintegratedCombinedMeasurements cpim(combined_params, currentBias);
+  cpim.integrateMeasurement(accMeas, omegaMeas, deltaT);
+
+  // Assert if the noise covariance
+  EXPECT(assert_equal(pim.preintMeasCov(),
+                      cpim.preintMeasCov().block(0, 0, 9, 9)));
+}
+
+/* ************************************************************************* */
+TEST(CombinedImuFactor, Accelerating) {
+  const double a = 0.2, v = 50;
+
+  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
+  // going in X The body itself has Z axis pointing down
+  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
+  const Point3 initial_position(10, 20, 0);
+  const Vector3 initial_velocity(v, 0, 0);
+
+  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
+                                      Vector3(a, 0, 0));
+
+  const double T = 3.0;  // seconds
+
+  CombinedScenarioRunner runner(scenario, testing::Params(), T / 10);
+
+  PreintegratedCombinedMeasurements pim = runner.integrate(T);
+  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
+
+  auto estimatedCov = runner.estimateCovariance(T, 100);
+  Eigen::Matrix<double, 15, 15> expected = pim.preintMeasCov();
+  EXPECT(assert_equal(estimatedCov, expected, 0.1));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp
index 92f4998a2..9987cca3f 100644
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@@ -122,7 +122,7 @@ class Class : public Point3 {
   enum {dimension = 3};
   using Point3::Point3;
   const Vector3& vector() const { return *this; }
-  inline static Class identity() { return Class(0,0,0); }
+  inline static Class Identity() { return Class(0,0,0); }
   double norm(OptionalJacobian<1,3> H = boost::none) const {
     return norm3(*this, H);
   }
@@ -285,7 +285,7 @@ TEST(Expression, compose2) {
 // Test compose with one arguments referring to constant rotation.
 TEST(Expression, compose3) {
   // Create expression
-  Rot3_ R1(Rot3::identity()), R2(3);
+  Rot3_ R1(Rot3::Identity()), R2(3);
   Rot3_ R3 = R1 * R2;
 
   // Check keys
diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
index 35c750408..502c62c11 100644
--- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp
+++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
@@ -217,7 +217,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
 
   // Setup prior factors
   // Note: If x0 is too far away from the origin (e.g. x=100) this test can fail.
-  Pose3 x0(Rot3::identity(), Vector3(10, -1, 1));
+  Pose3 x0(Rot3::Identity(), Vector3(10, -1, 1));
   auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
   graph.addPrior<Pose3>(X(0), x0, x0_noise);
 
@@ -241,7 +241,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
   // Initial values
   // Just offset the initial pose by 1m. This is what we are trying to optimize.
   Values initialEstimate;
-  Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0)));
+  Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1,0,0)));
   initialEstimate.insert(P(1), p1);
   initialEstimate.insert(P(2), p2);
   initialEstimate.insert(X(0), x0_initial);
diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp
index b4876b27e..b142b2009 100644
--- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp
@@ -69,7 +69,7 @@ SmartProjectionParams params(
 TEST(SmartProjectionRigFactor, Constructor) {
   using namespace vanillaRig;
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   SmartRigFactor::shared_ptr factor1(
       new SmartRigFactor(model, cameraRig, params));
 }
@@ -89,7 +89,7 @@ TEST(SmartProjectionRigFactor, Constructor2) {
 TEST(SmartProjectionRigFactor, Constructor3) {
   using namespace vanillaRig;
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   SmartRigFactor::shared_ptr factor1(
       new SmartRigFactor(model, cameraRig, params));
   factor1->add(measurement1, x1, cameraId1);
@@ -99,7 +99,7 @@ TEST(SmartProjectionRigFactor, Constructor3) {
 TEST(SmartProjectionRigFactor, Constructor4) {
   using namespace vanillaRig;
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   SmartProjectionParams params2(
       gtsam::HESSIAN,
       gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
@@ -112,7 +112,7 @@ TEST(SmartProjectionRigFactor, Constructor4) {
 TEST(SmartProjectionRigFactor, Equals) {
   using namespace vanillaRig;
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
 
   SmartRigFactor::shared_ptr factor1(
       new SmartRigFactor(model, cameraRig, params));
@@ -140,7 +140,7 @@ TEST(SmartProjectionRigFactor, noiseless) {
   Point2 level_uv_right = level_camera_right.project(landmark1);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
 
   SmartRigFactor factor(model, cameraRig, params);
   factor.add(level_uv, x1);  // both taken from the same camera
@@ -197,7 +197,7 @@ TEST(SmartProjectionRigFactor, noisy) {
   using namespace vanillaRig;
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
 
   // Project two landmarks into two cameras
   Point2 pixelError(0.2, 0.2);
@@ -323,7 +323,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
       Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
   Pose3 body_T_sensor2 =
       Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1));
-  Pose3 body_T_sensor3 = Pose3::identity();
+  Pose3 body_T_sensor3 = Pose3();
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
   cameraRig->push_back(Camera(body_T_sensor1, sharedK));
@@ -407,7 +407,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
   Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
+  cameraRig->push_back(Camera(Pose3(), sharedK2));
 
   // Project three landmarks into three cameras
   projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@@ -495,7 +495,7 @@ TEST(SmartProjectionRigFactor, Factors) {
   FastVector<size_t> cameraIds{0, 0};
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
 
   SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
       model, cameraRig, params);
@@ -578,7 +578,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
 
   // create smart factor
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   FastVector<size_t> cameraIds{0, 0, 0};
   SmartRigFactor::shared_ptr smartFactor1(
       new SmartRigFactor(model, cameraRig, params));
@@ -646,7 +646,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) {
   params.setEnableEPI(false);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   FastVector<size_t> cameraIds{0, 0, 0};
 
   SmartRigFactor::shared_ptr smartFactor1(
@@ -717,7 +717,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
   params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   FastVector<size_t> cameraIds{0, 0, 0};
 
   SmartRigFactor::shared_ptr smartFactor1(
@@ -783,7 +783,7 @@ TEST(SmartProjectionRigFactor, CheckHessian) {
   params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   FastVector<size_t> cameraIds{0, 0, 0};
 
   SmartRigFactor::shared_ptr smartFactor1(
@@ -859,7 +859,7 @@ TEST(SmartProjectionRigFactor, Hessian) {
   measurements_cam1.push_back(cam2_uv1);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
+  cameraRig->push_back(Camera(Pose3(), sharedK2));
   FastVector<size_t> cameraIds{0, 0};
 
   SmartProjectionParams params(
@@ -889,7 +889,7 @@ TEST(SmartProjectionRigFactor, Hessian) {
 TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
   using namespace bundlerPose;
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
+  cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
 
   SmartProjectionParams params;
   params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
@@ -917,7 +917,7 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) {
   KeyVector views{x1, x2, x3};
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
+  cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
   FastVector<size_t> cameraIds{0, 0, 0};
 
   SmartRigFactor::shared_ptr smartFactor1(
@@ -988,7 +988,7 @@ TEST(SmartProjectionRigFactor,
   KeyVector keys{x1, x2, x3, x1};
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   FastVector<size_t> cameraIds{0, 0, 0, 0};
 
   SmartRigFactor::shared_ptr smartFactor1(
@@ -1116,7 +1116,7 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
   // create inputs
   KeyVector keys{x1, x2, x3};
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3(), sharedK));
   FastVector<size_t> cameraIds{0, 0, 0};
   FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0};
 
@@ -1195,11 +1195,11 @@ TEST(SmartProjectionRigFactor, timing) {
   // Default cameras for simple derivatives
   static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
 
-  Rot3 R = Rot3::identity();
+  Rot3 R = Rot3::Identity();
   Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
   Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
   Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3();
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());  // single camera in the rig
   cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
@@ -1267,7 +1267,7 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
   keys.push_back(x3);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), emptyK));
+  cameraRig->push_back(Camera(Pose3(), emptyK));
 
   SmartProjectionParams params(
       gtsam::HESSIAN,
@@ -1330,10 +1330,10 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
 /* *************************************************************************/
 TEST(SmartProjectionFactorP, timing_sphericalCamera) {
   // create common data
-  Rot3 R = Rot3::identity();
+  Rot3 R = Rot3::Identity();
   Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
   Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3();
   Point3 landmark1(0, 0, 10);
 
   // create spherical data
@@ -1423,7 +1423,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
 
     boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
         new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
-    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
+    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), sharedK));
 
     SmartRigFactor::shared_ptr smartFactor1(
         new SmartRigFactor(model, cameraRig, params));
@@ -1461,7 +1461,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
 
     boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
             new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
-    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
+    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));
 
     SmartRigFactor::shared_ptr smartFactor1(
         new SmartRigFactor(model, cameraRig, params));
@@ -1498,7 +1498,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
 
     boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
         new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
-    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
+    cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));
 
     SmartRigFactor::shared_ptr smartFactor1(
         new SmartRigFactor(model, cameraRig, params));
@@ -1537,7 +1537,7 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
 
   boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
           new CameraSet<SphericalCamera>());  // single camera in the rig
-  cameraRig->push_back(SphericalCamera(Pose3::identity(), emptyK));
+  cameraRig->push_back(SphericalCamera(Pose3(), emptyK));
 
   // TRIANGULATION TEST WITH DEFAULT RANK TOL
   {  // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a
diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
index 6d8206177..b60b2f3ae 100644
--- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
+++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
@@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
 const double dt = 1.0;
 
 PoseRTV origin,
-        pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)),
+        pose1(Point3(0.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0)),
         pose1a(Point3(0.5, 0.0, 0.0)),
-        pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0));
+        pose2(Point3(1.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0));
 
 /* ************************************************************************* */
 TEST( testVelocityConstraint, trapezoidal ) {
diff --git a/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp
index ad96934c8..730d342f0 100644
--- a/gtsam_unstable/examples/GncPoseAveragingExample.cpp
+++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp
@@ -53,7 +53,7 @@ int main(int argc, char** argv){
   normal_distribution<double> normalInliers(0.0, 0.05);
 
   Values initial;
-  initial.insert(0, Pose3::identity()); // identity pose as initialization
+  initial.insert(0, Pose3()); // identity pose as initialization
 
   // create ground truth pose
   Vector6 poseGtVector;
diff --git a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
index c0a102d11..16eda23d4 100644
--- a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
+++ b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
@@ -129,8 +129,8 @@ int main(int argc, char* argv[]) {
   // Pose prior - at identity
   auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
-  graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);
-  initialEstimate.insert(X(0), Pose3::identity());
+  graph.addPrior(X(1), Pose3::Identity(), priorPoseNoise);
+  initialEstimate.insert(X(0), Pose3::Identity());
 
   // Bias prior
   graph.addPrior(B(1), imu.priorImuBias,
@@ -157,7 +157,7 @@ int main(int argc, char* argv[]) {
     if (frame != lastFrame || in.eof()) {
       cout << "Running iSAM for frame: " << lastFrame << "\n";
 
-      initialEstimate.insert(X(lastFrame), Pose3::identity());
+      initialEstimate.insert(X(lastFrame), Pose3::Identity());
       initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
       initialEstimate.insert(B(lastFrame), imu.prevBias);
 
diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h
index d833c9cf4..037a6cc9d 100644
--- a/gtsam_unstable/geometry/Pose3Upright.h
+++ b/gtsam_unstable/geometry/Pose3Upright.h
@@ -95,7 +95,7 @@ public:
   /// @{
 
   /// identity for group operation
-  static Pose3Upright identity() { return Pose3Upright(); }
+  static Pose3Upright Identity() { return Pose3Upright(); }
 
   /// inverse transformation with derivatives
   Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const;
diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i
index 08cd45e18..c6dbd4ab6 100644
--- a/gtsam_unstable/gtsam_unstable.i
+++ b/gtsam_unstable/gtsam_unstable.i
@@ -130,7 +130,7 @@ class Pose3Upright {
   gtsam::Pose3Upright retract(Vector v) const;
   Vector localCoordinates(const gtsam::Pose3Upright& p2) const;
 
-  static gtsam::Pose3Upright identity();
+  static gtsam::Pose3Upright Identity();
   gtsam::Pose3Upright inverse() const;
   gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
   gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;
diff --git a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
index b97d56c7e..c2b526ecd 100644
--- a/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
+++ b/gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
@@ -44,8 +44,8 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) {
 
   // Init pose and prior.  Pose Prior is needed since a single plane measurement
   // does not fully constrain the pose
-  Pose3 init_pose = Pose3::identity();
-  Pose3 anchor_pose = Pose3::identity();
+  Pose3 init_pose = Pose3::Identity();
+  Pose3 anchor_pose = Pose3::Identity();
   graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
   graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001));
 
@@ -89,7 +89,7 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
 
   // Init pose and prior.  Pose Prior is needed since a single plane measurement
   // does not fully constrain the pose
-  Pose3 init_pose = Pose3::identity();
+  Pose3 init_pose = Pose3::Identity();
   graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
 
   // Add two landmark measurements, differing in angle
@@ -180,8 +180,8 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
   NonlinearFactorGraph graph;
 
   // Setup prior factors
-  Pose3 x0(Rot3::identity(), Vector3(100, 30, 10));  // the "sensor pose"
-  Pose3 x1(Rot3::identity(), Vector3(90, 40,  5) );  // the "anchor pose"
+  Pose3 x0(Rot3::Identity(), Vector3(100, 30, 10));  // the "sensor pose"
+  Pose3 x1(Rot3::Identity(), Vector3(90, 40,  5) );  // the "anchor pose"
 
   auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
   auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01);
@@ -213,7 +213,7 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
   // Initial values
   // Just offset the initial pose by 1m. This is what we are trying to optimize.
   Values initialEstimate;
-  Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1, 0, 0)));
+  Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1, 0, 0)));
   initialEstimate.insert(P(1), p1_in_x1);
   initialEstimate.insert(P(2), p2_in_x1);
   initialEstimate.insert(X(0), x0_initial);
diff --git a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
index 7ba8a0d04..615ed13aa 100644
--- a/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
@@ -173,7 +173,7 @@ TEST(PartialPriorFactor, Constructors3) {
 /* ************************************************************************* */
 TEST(PartialPriorFactor, JacobianAtIdentity3) {
   Key poseKey(1);
-  Pose3 measurement = Pose3::identity();
+  Pose3 measurement = Pose3::Identity();
   SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);
 
   TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp b/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp
index 5aaaaec53..7345e2308 100644
--- a/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.cpp
@@ -16,7 +16,7 @@ using namespace gtsam::noiseModel;
 /* ************************************************************************* */
 // Verify zero error when there is no noise
 TEST(PoseToPointFactor, errorNoiseless_2D) {
-  Pose2 pose = Pose2::identity();
+  Pose2 pose = Pose2::Identity();
   Point2 point(1.0, 2.0);
   Point2 noise(0.0, 0.0);
   Point2 measured = point + noise;
@@ -33,7 +33,7 @@ TEST(PoseToPointFactor, errorNoiseless_2D) {
 /* ************************************************************************* */
 // Verify expected error in test scenario
 TEST(PoseToPointFactor, errorNoise_2D) {
-  Pose2 pose = Pose2::identity();
+  Pose2 pose = Pose2::Identity();
   Point2 point(1.0, 2.0);
   Point2 noise(-1.0, 0.5);
   Point2 measured = point + noise;
@@ -86,7 +86,7 @@ TEST(PoseToPointFactor, jacobian_2D) {
 /* ************************************************************************* */
 // Verify zero error when there is no noise
 TEST(PoseToPointFactor, errorNoiseless_3D) {
-  Pose3 pose = Pose3::identity();
+  Pose3 pose = Pose3::Identity();
   Point3 point(1.0, 2.0, 3.0);
   Point3 noise(0.0, 0.0, 0.0);
   Point3 measured = point + noise;
@@ -103,7 +103,7 @@ TEST(PoseToPointFactor, errorNoiseless_3D) {
 /* ************************************************************************* */
 // Verify expected error in test scenario
 TEST(PoseToPointFactor, errorNoise_3D) {
-  Pose3 pose = Pose3::identity();
+  Pose3 pose = Pose3::Identity();
   Point3 point(1.0, 2.0, 3.0);
   Point3 noise(-1.0, 0.5, 0.3);
   Point3 measured = point + noise;
diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
index b5962d777..b88a27c6c 100644
--- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
+++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
@@ -88,7 +88,7 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
 TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
   using namespace vanillaPoseRS;
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
   SmartFactorRS::shared_ptr factor1(
       new SmartFactorRS(model, cameraRig, params));
 }
@@ -97,7 +97,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
 TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
   using namespace vanillaPoseRS;
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
   params.setRankTolerance(rankTol);
   SmartFactorRS factor1(model, cameraRig, params);
 }
@@ -106,7 +106,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
 TEST(SmartProjectionPoseFactorRollingShutter, add) {
   using namespace vanillaPoseRS;
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
   SmartFactorRS::shared_ptr factor1(
       new SmartFactorRS(model, cameraRig, params));
   factor1->add(measurement1, x1, x2, interp_factor);
@@ -230,7 +230,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
   // Project two landmarks into two cameras
   Point2 level_uv = cam1.project(landmark1);
   Point2 level_uv_right = cam2.project(landmark1);
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3::Identity();
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
   cameraRig->push_back(Camera(body_P_sensorId, sharedK));
@@ -405,7 +405,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
   interp_factors.push_back(interp_factor3);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
 
   SmartFactorRS::shared_ptr smartFactor1(
       new SmartFactorRS(model, cameraRig, params));
@@ -480,7 +480,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
   cameraRig->push_back(Camera(body_P_sensor, sharedK));
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
 
   SmartFactorRS::shared_ptr smartFactor1(
       new SmartFactorRS(model, cameraRig, params));
@@ -633,11 +633,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
   // Default cameras for simple derivatives
   static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
 
-  Rot3 R = Rot3::identity();
+  Rot3 R = Rot3::Identity();
   Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
   Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
   Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3::Identity();
 
   // one landmarks 1m in front of camera
   Point3 landmark1(0, 0, 10);
@@ -747,7 +747,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
   params.setEnableEPI(true);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
 
   SmartFactorRS smartFactor1(model, cameraRig, params);
   smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
@@ -816,7 +816,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   params.setEnableEPI(false);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
 
   SmartFactorRS smartFactor1(model, cameraRig, params);
   smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
@@ -894,7 +894,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   params.setEnableEPI(false);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
 
   SmartFactorRS::shared_ptr smartFactor1(
       new SmartFactorRS(model, cameraRig, params));
@@ -961,7 +961,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   interp_factors.push_back(interp_factor3);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
 
   SmartFactorRS::shared_ptr smartFactor1(
       new SmartFactorRS(model, cameraRig, params));
@@ -1102,7 +1102,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   interp_factors.push_back(interp_factor1);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
 
   SmartFactorRS::shared_ptr smartFactor1(
       new SmartFactorRS(model, cameraRig, params));
@@ -1261,7 +1261,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
       interp_factors.at(0));  // we readd the first interp factor
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), sharedK));
+  cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
 
   SmartFactorRS::shared_ptr smartFactor1(
       new SmartFactorRS(model, cameraRig, params));
@@ -1331,11 +1331,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
       gtsam::HESSIAN,
       gtsam::ZERO_ON_DEGENERACY);  // only config that works with RS factors
 
-  Rot3 R = Rot3::identity();
+  Rot3 R = Rot3::Identity();
   Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
   Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
   Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
-  Pose3 body_P_sensorId = Pose3::identity();
+  Pose3 body_P_sensorId = Pose3::Identity();
 
   // one landmarks 1m in front of camera
   Point3 landmark1(0, 0, 10);
@@ -1431,7 +1431,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
   params.setRankTolerance(0.1);
 
   boost::shared_ptr<Cameras> cameraRig(new Cameras());
-  cameraRig->push_back(Camera(Pose3::identity(), emptyK));
+  cameraRig->push_back(Camera(Pose3::Identity(), emptyK));
 
   SmartFactorRS_spherical::shared_ptr smartFactor1(
       new SmartFactorRS_spherical(model, cameraRig, params));
diff --git a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
index 107defb4c..98b18371f 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
@@ -198,10 +198,10 @@ TEST(testISAM2SmartFactor, Stereo_Batch) {
 
     // prior, for the first keyframe:
     if (kf_id == 0) {
-      batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
+      batch_graph.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
     }
 
-    batch_values.insert(X(kf_id), Pose3::identity());
+    batch_values.insert(X(kf_id), Pose3::Identity());
   }
 
   LevenbergMarquardtParams parameters;
@@ -267,7 +267,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
   // Storage of smart factors:
   std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
 
-  Pose3 lastKeyframePose = Pose3::identity();
+  Pose3 lastKeyframePose = Pose3::Identity();
 
   // Run one timestep at once:
   for (const auto &entries : dataset) {
@@ -307,7 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
 
     // prior, for the first keyframe:
     if (kf_id == 0) {
-      newFactors.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
+      newFactors.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
     }
 
     // 2) Run iSAM2:
diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
index c71c19038..1dbd25666 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
@@ -181,8 +181,8 @@ TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_identityExtrinsics )
   Values values;
   values.insert(x1, w_Pose_cam1);
   values.insert(x2, w_Pose_cam2);
-  values.insert(body_P_cam1_key, Pose3::identity());
-  values.insert(body_P_cam2_key, Pose3::identity());
+  values.insert(body_P_cam1_key, Pose3::Identity());
+  values.insert(body_P_cam2_key, Pose3::Identity());
 
   SmartStereoProjectionFactorPP factor1(model);
   factor1.add(cam1_uv, x1, body_P_cam1_key, K2);
@@ -426,7 +426,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_multipleExtrinsics ) {
   // Values
   Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
   Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),Point3(1, 1, 1));
-  Pose3 body_Pose_cam3 = Pose3::identity();
+  Pose3 body_Pose_cam3 = Pose3::Identity();
   Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
   Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
   Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse());
@@ -1147,7 +1147,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
   graph.push_back(smartFactor3);
   graph.addPrior(x1, pose1, noisePrior);
   graph.addPrior(x2, pose2, noisePrior);
-  graph.addPrior(body_P_cam_key, Pose3::identity(), noisePrior);
+  graph.addPrior(body_P_cam_key, Pose3::Identity(), noisePrior);
 
   //  Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@@ -1156,7 +1156,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
   values.insert(x1, pose1);
   values.insert(x2, pose2);
   values.insert(x3, pose3 * noise_pose);
-  values.insert(body_P_cam_key, Pose3::identity());
+  values.insert(body_P_cam_key, Pose3::Identity());
 
   // All smart factors are disabled and pose should remain where it is
   Values result;
@@ -1245,7 +1245,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
   values.insert(x1, pose1);
   values.insert(x2, pose2);
   values.insert(x3, pose3);
-  values.insert(body_P_cam_key, Pose3::identity());
+  values.insert(body_P_cam_key, Pose3::Identity());
 
   EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
   EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
@@ -1267,7 +1267,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
   result = optimizer.optimize();
-  EXPECT(assert_equal(Pose3::identity(), result.at<Pose3>(body_P_cam_key)));
+  EXPECT(assert_equal(Pose3::Identity(), result.at<Pose3>(body_P_cam_key)));
 }
 
 /* ************************************************************************* */
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index 6d23144aa..5b27eea4d 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -382,7 +382,7 @@ TEST(ExpressionFactor, compose2) {
 TEST(ExpressionFactor, compose3) {
 
   // Create expression
-  Rot3_ R1(Rot3::identity()), R2(3);
+  Rot3_ R1(Rot3::Identity()), R2(3);
   Rot3_ R3 = R1 * R2;
 
   // Create factor
diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp
index ac3a09e36..a05c4b621 100644
--- a/tests/testManifold.cpp
+++ b/tests/testManifold.cpp
@@ -139,7 +139,7 @@ TEST(Manifold, DefaultChart) {
 
   Vector v3(3);
   v3 << 1, 1, 1;
-  Rot3 I = Rot3::identity();
+  Rot3 I = Rot3::Identity();
   Rot3 R = I.retract(v3);
   //DefaultChart<Rot3> chart5;
   EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
diff --git a/timing/timeShonanFactor.cpp b/timing/timeShonanFactor.cpp
index 207d54a4d..60dd6f47a 100644
--- a/timing/timeShonanFactor.cpp
+++ b/timing/timeShonanFactor.cpp
@@ -62,9 +62,9 @@ int main(int argc, char* argv[]) {
 
   // Build graph
   NonlinearFactorGraph graph;
-  // graph.add(NonlinearEquality<SOn>(0, SOn::identity(4)));
+  // graph.add(NonlinearEquality<SOn>(0, SOn::Identity(4)));
   auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
-  graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel));
+  graph.add(PriorFactor<SOn>(0, SOn::Identity(4), priorModel));
   auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4));
   for (const auto &m : measurements) {
     const auto &keys = m.keys();
@@ -92,7 +92,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < 100; i++) {
     gttic_(optimize);
     Values initial;
-    initial.insert(0, SOn::identity(4));
+    initial.insert(0, SOn::Identity(4));
     for (size_t j = 1; j < poses.size(); j++) {
       initial.insert(j, SOn::Random(rng, 4));
     }