John L. Crassidis, and John L. Junkins, Optimal Estimation of Dynamic Systems, CRC Press, 2011.
Corrections to the book can be found at here .
Chapter 2 Probability Concepts in Least Squares
2.5. Maximum Likelihood Estimation
2.6. Properties of Maximum Likelihood Estimation
Invariance Principle
Consistent Estimator
Asymptotically Gaussian Property
Asymptotically Efficient Property
2.7. Bayesian Estimation
2.7.1. MAP Estimation
Maximum a posteriori estimation (MAP)
if the a priori distribution p ( x ^ ) p(\hat{\bm{x}}) p ( x ^ ) is uniform, then MAP estimation is equivalent to maximum likelihood (MLL) estimation.
asymptotic consistency and efficiency
MAP estimator converges to MLL estimator for large samples
obeys the invariance principle
2.7.2. Minimum Risk Estimation
assume x \bm{x} x is distributed by the a posteriori function p ( x ∣ y ~ ) p(\bm{x}|\tilde{\bm{y}}) p ( x ∣ y ~ )
assign a cost to any loss suffered due to errors in the estimate
The risk funtion is
J M R ( x ∗ ) = ∫ − ∞ ∞ c ( x ∗ ∣ x ) p ( x ∣ y ~ ) d x (2.171) J_{\rm MR}(\bm{x}^*) = \int_{-\infty}^{\infty} c(\bm{x}^*|\bm{x}) p(\bm{x}|\tilde{\bm{y}}) d\bm{x} \tag{2.171}
J M R ( x ∗ ) = ∫ − ∞ ∞ c ( x ∗ ∣ x ) p ( x ∣ y ~ ) d x ( 2 . 1 7 1 )
[WHAT’S THE RELATION TO GP ?? LOOKS LIKE A PREDICTIVE DISTRIBUTION.]
For these reasons (see p97), minimum risk approaches are often avoided in practical estimation problems, although the relationship between decision theory and optimal estimation is very interesting.
2.8. Advanced Topics (not read yet)
2.8.1. Nonuniqueness of the Weight Matrix
2.8.2. Analysis of Covariance Errors
2.8.3. Ridge Estimation
2.8.4. Total Least Squares
3. Sequential State EStimation
3.2. Full-Order Estimators
Ackermann’s formula
3.2.1. Discrete-Time Estimators
Discrete state-space representation
x k + 1 = Φ x k + Γ u k y k = H x k + D u k (A.122) \begin{aligned}
\bm{x}_{k+1} &= \Phi \bm{x}_k + \Gamma \bm{u}_k\\
\bm{y}_k &= H \bm{x}_k + D \bm{u}_k
\end{aligned}
\tag{A.122}
x k + 1 y k = Φ x k + Γ u k = H x k + D u k ( A . 1 2 2 )
x ^ k + 1 − = Φ x ^ k + + Γ u k x ^ k + = x ^ k − + K [ y ~ − H x ^ k − ] (3.20) \begin{aligned}
\hat{\bm{x}}_{k+1}^- &= \Phi \hat{\bm{x}}_k^+ + \Gamma\bm{u}_k\\
\hat{\bm{x}}_k^+ &= \hat{\bm{x}}_k^- + K [\tilde{\bm{y}}-H\hat{\bm{x}}_k^-]
\end{aligned}
\tag{3.20}
x ^ k + 1 − x ^ k + = Φ x ^ k + + Γ u k = x ^ k − + K [ y ~ − H x ^ k − ] ( 3 . 2 0 )
3.3. The Discrete-Time Kalman Filter
The Kalman filter provides a rigorous theoretical approach to “place” the poles of the estimator, based upon stochastic processes for the measurement error and model error.
both discrete-time dynamic models and measurements
both continuous-time dynamic models and measurements
continuous-time dynamic models with discrete-time measurements
3.3.1. Kalman Filter Derivation
[INSERT TABLE 3.1 HERE]
x k + 1 = Φ k x k + Γ k u k + Υ k w k (3.27a) \bm{x}_{k+1} = \Phi_k \bm{x}_k + \Gamma_k \bm{u}_k + \Upsilon_k \bm{w}_k
\tag{3.27a}
x k + 1 = Φ k x k + Γ k u k + Υ k w k ( 3 . 2 7 a )
y ~ k = H k x k + v k (3.27b) \tilde{\bm{y}}_k = H_k \bm{x}_k + \bm{v}_k
\tag{3.27b}
y ~ k = H k x k + v k ( 3 . 2 7 b )
assuming the errors are not correlated forward or backward in time, so that
E { v k v j T } = { 0 k ≠ j R k k = j (3.28) E\{ \bm{v}_k \bm{v}_j^T \} = \begin{cases}
0 & k \neq j \\
R_k & k = j
\end{cases}
\tag{3.28}
E { v k v j T } = { 0 R k k = j k = j ( 3 . 2 8 )
and
E { w k w j T } = { 0 k ≠ j Q k k = j (3.29) E\{ \bm{w}_k \bm{w}_j^T \} = \begin{cases}
0 & k \neq j \\
Q_k & k = j
\end{cases}
\tag{3.29}
E { w k w j T } = { 0 Q k k = j k = j ( 3 . 2 9 )
x ~ k + 1 − = Φ k x ~ k + − Υ k w k (3.33) \tilde{\bm{x}}_{k+1}^- = \Phi_k \tilde{\bm{x}}_k^+ - \Upsilon_k \bm{w}_k
\tag{3.33}
x ~ k + 1 − = Φ k x ~ k + − Υ k w k ( 3 . 3 3 )
P k + 1 − ≡ E { x ~ k + 1 − x ~ k + 1 − T } = E { Φ k x ~ k + x ~ k + T Φ k T } + E { Υ k w k w k T Υ k T } = − E { Φ k x ~ k + w k T Υ k T } − E { Υ k w k x ~ k + T Φ k T } (3.34) \newcommand{\xm}[1]{\tilde{\bm{x}}_{ #1}^{-}}
\newcommand{\xmT}[1]{\tilde{\bm{x}}_{ #1}^{-T}}
\newcommand{\xp}[1]{\tilde{\bm{x}}_{ #1}^{+}}
\newcommand{\xpT}[1]{\tilde{\bm{x}}_{ #1}^{+T}}
\newcommand{\wk}{\bm{w}_k}
\newcommand{\wkT}{\bm{w}_k^T}
\newcommand{\E}[1]{E\{ #1 \}}
\begin{aligned}
P_{k+1}^-
&\equiv E\{ \xm{k+1} \xmT{k+1} \} \\
&= \E{ \Phi_k \xp{k}\xpT{k} \Phi_k^T } + \E{\Upsilon_k\wk\wkT\Upsilon_k^T} \\
&\phantom{=} - \E{\Phi_k\xp{k}\wkT\Upsilon_k^T} - \E{\Upsilon_k\wk\xpT{k}\Phi_k^T} \\
\end{aligned}
\tag{3.34}
P k + 1 − ≡ E { x ~ k + 1 − x ~ k + 1 − T } = E { Φ k x ~ k + x ~ k + T Φ k T } + E { Υ k w k w k T Υ k T } = − E { Φ k x ~ k + w k T Υ k T } − E { Υ k w k x ~ k + T Φ k T } ( 3 . 3 4 )
where w k + \bm{w}_k^+ w k + and x ~ k + \tilde{\bm{x}}_k^+ x ~ k + are uncorrelated since x ~ k + 1 − \tilde{\bm{x}}_{k+1}^- x ~ k + 1 − (not x ~ k \tilde{\bm{x}}_k x ~ k ) directly depends on w k \bm{w}_k w k .
P k + 1 − = Φ k P k + Φ k T − Υ k Q k Υ k T (3.35) P_{k+1}^- = \Phi_k P_k^+ \Phi_k^T - \Upsilon_k Q_k \Upsilon_k^T
\tag{3.35}
P k + 1 − = Φ k P k + Φ k T − Υ k Q k Υ k T ( 3 . 3 5 )
x ~ k + = ( I − K k H k ) x ~ k − + K k v k (3.37) \tilde{\bm{x}}_k^+ = (I - K_k H_k) \tilde{\bm{x}}_k^- + K_k \bm{v}_k
\tag{3.37}
x ~ k + = ( I − K k H k ) x ~ k − + K k v k ( 3 . 3 7 )
P k + ≡ E { x ~ k + x ~ k + T } = E { ( I − K k H k ) x ~ k − x ~ k − T ( I − K k H k ) T } + E { K k v k v k T K k T } = + E { ( I − K k H k ) x ~ k − v k T K k T } + E { K k v k x ~ k − T ( I − K k H k ) T } (3.38) \newcommand{\xm}[1]{\tilde{\bm{x}}_{ #1}^{-}}
\newcommand{\xmT}[1]{\tilde{\bm{x}}_{ #1}^{-T}}
\newcommand{\xp}[1]{\tilde{\bm{x}}_{ #1}^{+}}
\newcommand{\xpT}[1]{\tilde{\bm{x}}_{ #1}^{+T}}
\newcommand{\wk}{\bm{w}_k}
\newcommand{\wkT}{\bm{w}_k^T}
\newcommand{\E}[1]{E\{ #1 \}}
\begin{aligned}
P_k^+
&\equiv \E{ \xp{k}\xpT{k} } \\
&= \E{ (I - K_k H_k) \xm{k} \xmT{k} (I - K_k H_k)^T } + \E{ K_k\bm{v}_k\bm{v}_k^TK_k^T } \\
&\phantom{=} + \E{(I - K_k H_k) \xm{k} \bm{v}_k^T K_k^T} + \E{ K_k \bm{v}_k \xmT{k} (I - K_k H_k)^T }
\end{aligned}
\tag{3.38}
P k + ≡ E { x ~ k + x ~ k + T } = E { ( I − K k H k ) x ~ k − x ~ k − T ( I − K k H k ) T } + E { K k v k v k T K k T } = + E { ( I − K k H k ) x ~ k − v k T K k T } + E { K k v k x ~ k − T ( I − K k H k ) T } ( 3 . 3 8 )
Another form for P k + 1 P_{k+1} P k + 1 that ensures to be positive definite:
Using
P k + = [ I − K k H k ] P k − [ I − k k H k ] T + K k R k K k T (3.39) P_k^+ = [I-K_kH_k]P_k^-[I-k_kH_k]^T + K_kR_kK_k^T \tag{3.39}
P k + = [ I − K k H k ] P k − [ I − k k H k ] T + K k R k K k T ( 3 . 3 9 )
instead of
P k + = [ I − K k H k ] P k − (3.44) P_k^+ = [I-K_kH_k]P_k^- \tag{3.44}
P k + = [ I − K k H k ] P k − ( 3 . 4 4 )
Proof: see p149–151.
handle large measurement vectors
The information filter may be more computationally efficient than the traditional Kalman filter when the size of the measurement vector is much larger than the size of the state vector.
Another more commonly used approach is to use sequential processing.
processing one measurement at a time, then the tain and covariance are updated until all measurements at each sampling instant have been processed.
3.3.4. Steady-State Kalman Filter
For time-invariant systems the error covariance P P P reaches a steady-state value very quickly.
Therefore, a constant gain K K K can be pre-computed using the steady-state covariance.
The solution is suboptimal in the strictest sense but can save a lot in computations.
3.3.5. Relationship to Least Squares Estimation
The Kalman filter can be derived using a least squares type loss function
J = 1 2 ( x ^ 0 − x 0 ) T P a 0 ( x ^ 0 − x 0 ) + 1 2 ∑ i = 1 k ( y ~ − H i x ^ i ) T R i − 1 ( y ~ − H i x ^ i ) (3.109) J = \frac{1}{2}(\hat{\bm{x}}_0-\bm{x}_0)^T\mathcal{Pa}_0(\hat{\bm{x}}_0-\bm{x}_0)
+ \frac{1}{2} \sum_{i=1}^k (\tilde{\bm{y}}-H_i\hat{\bm{x}}_i)^T R_i^{-1} (\tilde{\bm{y}}-H_i\hat{\bm{x}}_i) \tag{3.109}
J = 2 1 ( x ^ 0 − x 0 ) T P a 0 ( x ^ 0 − x 0 ) + 2 1 i = 1 ∑ k ( y ~ − H i x ^ i ) T R i − 1 ( y ~ − H i x ^ i ) ( 3 . 1 0 9 )
subject to the constraint
x ^ i + 1 = Φ ( i + 1 , i ) x ^ i , i = 1 , 2 , … , k − 1 (3.110) \hat{\bm{x}}_{i+1} = \Phi(i+1,i)\hat{\bm{x}}_i,\quad i=1,2,\dots,k-1 \tag{3.110}
x ^ i + 1 = Φ ( i + 1 , i ) x ^ i , i = 1 , 2 , … , k − 1 ( 3 . 1 1 0 )
Consider the correlation between w k − 1 \bm{w}_{k-1} w k − 1 and v k \bm{v}_k v k
E { w k − 1 v k T } = S k (3.127) E\{ \bm{w}_{k-1} \bm{v}_k^T \} = S_k
\tag{3.127}
E { w k − 1 v k T } = S k ( 3 . 1 2 7 )
because only w k − 1 \bm{w}_{k-1} w k − 1 and v k \bm{v}_k v k affect the measurement at the same time (3.27a substituted into 3.27b), as shown below,
y ~ k = H k x k + v k = H k ( Φ k − 1 x k − 1 + Γ k − 1 u k − 1 + Υ k − 1 w k − 1 ) + v k . \begin{aligned}
\tilde{\bm{y}}_k &= H_k \bm{x}_k + \bm{v}_k \\
&= H_k \left( \Phi_{k-1} \bm{x}_{k-1} + \Gamma_{k-1} \bm{u}_{k-1} + \Upsilon_{k-1} \bm{w}_{k-1} \right) + \bm{v}_k.
\end{aligned}
y ~ k = H k x k + v k = H k ( Φ k − 1 x k − 1 + Γ k − 1 u k − 1 + Υ k − 1 w k − 1 ) + v k .
In Eq. (3.38),
P k + ≡ E { x ~ k + x ~ k + T } = E { ( I − K k H k ) x ~ k − x ~ k − T ( I − K k H k ) T } + E { K k v k v k T K k T } = + E { ( I − K k H k ) x ~ k − v k T K k T } + E { K k v k x ~ k − T ( I − K k H k ) T } , (3.38) \newcommand{\xm}[1]{\tilde{\bm{x}}_{ #1}^{-}}
\newcommand{\xmT}[1]{\tilde{\bm{x}}_{ #1}^{-T}}
\newcommand{\xp}[1]{\tilde{\bm{x}}_{ #1}^{+}}
\newcommand{\xpT}[1]{\tilde{\bm{x}}_{ #1}^{+T}}
\newcommand{\wk}{\bm{w}_k}
\newcommand{\wkT}{\bm{w}_k^T}
\newcommand{\E}[1]{E\{ #1 \}}
\begin{aligned}
P_k^+
&\equiv \E{ \xp{k}\xpT{k} } \\
&= \E{ (I - K_k H_k) \xm{k} \xmT{k} (I - K_k H_k)^T } + \E{ K_k\bm{v}_k\bm{v}_k^TK_k^T } \\
&\phantom{=} + \E{(I - K_k H_k) \xm{k} \bm{v}_k^T K_k^T} + \E{ K_k \bm{v}_k \xmT{k} (I - K_k H_k)^T },
\end{aligned}
\tag{3.38}
P k + ≡ E { x ~ k + x ~ k + T } = E { ( I − K k H k ) x ~ k − x ~ k − T ( I − K k H k ) T } + E { K k v k v k T K k T } = + E { ( I − K k H k ) x ~ k − v k T K k T } + E { K k v k x ~ k − T ( I − K k H k ) T } , ( 3 . 3 8 )
the following expectation is not zero:
E { x ~ k − v k T } = E { ( Φ k − 1 x ~ k − 1 + − Υ k − 1 w k − 1 ) v k T } = − E { Υ k − 1 w k − 1 v k T } = − Υ k − 1 S k (3.128) E\{\tilde{\bm{x}}_k^-\bm{v}_k^T\} = E\{ \left( \Phi_{k-1}\tilde{\bm{x}}_{k-1}^+ - \Upsilon_{k-1}\bm{w}_{k-1} \right) \bm{v}_k^T \}
= - E\{ \Upsilon_{k-1}\bm{w}_{k-1} \bm{v}_k^T \} = - \Upsilon_{k-1} S_k
\tag{3.128}
E { x ~ k − v k T } = E { ( Φ k − 1 x ~ k − 1 + − Υ k − 1 w k − 1 ) v k T } = − E { Υ k − 1 w k − 1 v k T } = − Υ k − 1 S k ( 3 . 1 2 8 )
This leads to different covariance update
P k + = … ( Only use its trace to derive K k ) (3.129) P_{k}^+ = \dots \quad \color{red} (\text{Only use its trace to derive } K_k)
\tag{3.129}
P k + = … ( Only use its trace to derive K k ) ( 3 . 1 2 9 )
and Kalman gain (still by minimizing the above P k + P_{k}^+ P k + )
K k = … (3.130) K_k = \dots
\tag{3.130}
K k = … ( 3 . 1 3 0 )
which could simplify Eq. (3.129) to the final covariance update formula
P k + = … ( Use this one for propagation ) P_k^+ = \dots \quad \color{red} (\text{Use this one for propagation})
P k + = … ( Use this one for propagation )
(AN APPLICATION EXAMPLE, refined by me)
an aircraft flying through a field of random turbulence.
The effect of turbulence in the aircraft’s acceleration is modeled as process noise on w k − 1 \bm{w}_{k-1} w k − 1 .
Since any sensor mounted on an aircraft is also corrupted by turbulence, the measurement error v k \bm{v}_k v k is correlated with the process noise w k − 1 \bm{w}_{k-1} w k − 1 .
3.3.7. Cramer-Rao Lower Bound
see wikipedia :
In its simplest form, the bound states that the variance of any unbiased estimator is at least as high as the inverse of the Fisher information.\
3.3.8. Orthogonality Principle
the orthogonality of the estimate and its error
E { x ^ k + x ~ k + T } = 0 (3.152) E\{\hat{\bm{x}}_k^+\tilde{\bm{x}}_k^{+T}\} = 0 \tag{3.152}
E { x ^ k + x ~ k + T } = 0 ( 3 . 1 5 2 )
where
x ~ k + ≡ x ^ k + − x k (3.32) \tilde{\bm{x}}_k^+ \equiv \hat{\bm{x}}_k^+ - \bm{x}_k \tag{3.32}
x ~ k + ≡ x ^ k + − x k ( 3 . 3 2 )
This orthogonality principle is extremely important in the derivation of the linear quadratic-Gaussian controller of Sec. 8.6.
Example 3.3: derive the process noise
For the real system (where there is always a random process term) :
Continuous system:
θ ˙ = ω ~ − β − η v β ˙ = η u \begin{aligned}
\dot{\theta} &= \tilde{\omega} - \beta - \eta_v \\
\dot{\beta} &= \eta_u
\end{aligned}
θ ˙ β ˙ = ω ~ − β − η v = η u
Discrete system:
Let Δ t = t k + 1 − t k \Delta t = t_{k+1} - t_{k} Δ t = t k + 1 − t k .
θ k + 1 − θ k = ∫ t k t k + 1 [ ω ~ − β − η v ] d τ = ∫ t k t k + 1 ω ~ d τ − ∫ t k t k + 1 β ( τ ) d τ − ∫ t k t k + 1 η v d τ = ∫ t k t k + 1 ω ~ d τ − [ β k Δ t + ∫ t k t k + 1 ( t k + 1 − τ ) η u ( τ ) d τ ] − ∫ t k t k + 1 η v d τ [see Eq. eg.3.3.x1] = ∫ t k t k + 1 ω ~ d τ − β k Δ t − ∫ t k t k + 1 [ η v ( τ ) + ( t k + 1 − τ ) η u ( τ ) ] d τ β k + 1 − β k = ∫ t k t k + 1 η u d τ ⇒ β ( τ ) = β k + ∫ t k τ η u ( ν ) d ν \begin{aligned}
\theta_{k+1} - \theta_{k} &= \int_{t_k}^{t_{k+1}} [\tilde{\omega} - \beta - \eta_v] d\tau \\
&= \int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau - \int_{t_k}^{t_{k+1}} \textcolor{red}{\beta(\tau)} d\tau - \int_{t_k}^{t_{k+1}} \eta_v d\tau\\
&= \int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau
- \left[ \beta_k \Delta t + \int_{t_k}^{t_{k+1}} (t_{k+1} - \tau) \eta_u(\tau)d\tau \right]
- \int_{t_k}^{t_{k+1}} \eta_v d\tau \quad \text{[see Eq. eg.3.3.x1]}\\
&= \int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau
- \beta_k \Delta t - \int_{t_k}^{t_{k+1}} \left[ \eta_v(\tau) + (t_{k+1} - \tau)\eta_u(\tau) \right] d\tau \\
\\
\beta_{k+1} - \beta_{k} &= \int_{t_k}^{t_{k+1}} \eta_u d\tau
\quad\Rightarrow\quad \textcolor{red}{\beta(\tau) = \beta_k + \int_{t_k}^{\tau} \eta_u(\nu) d\nu} \\
\end{aligned}
θ k + 1 − θ k β k + 1 − β k = ∫ t k t k + 1 [ ω ~ − β − η v ] d τ = ∫ t k t k + 1 ω ~ d τ − ∫ t k t k + 1 β ( τ ) d τ − ∫ t k t k + 1 η v d τ = ∫ t k t k + 1 ω ~ d τ − [ β k Δ t + ∫ t k t k + 1 ( t k + 1 − τ ) η u ( τ ) d τ ] − ∫ t k t k + 1 η v d τ [see Eq. eg.3.3.x1] = ∫ t k t k + 1 ω ~ d τ − β k Δ t − ∫ t k t k + 1 [ η v ( τ ) + ( t k + 1 − τ ) η u ( τ ) ] d τ = ∫ t k t k + 1 η u d τ ⇒ β ( τ ) = β k + ∫ t k τ η u ( ν ) d ν
∫ t k t k + 1 β ( τ ) d τ = ∫ t k t k + 1 [ β k + ∫ t k τ η u ( ν ) d ν ] d τ = ∫ t k t k + 1 β k d τ + ∫ t k t k + 1 ∫ t k τ η u ( τ ) d τ d τ = β k Δ t + ∫ t k t k + 1 ∫ t k τ η u ( ν ) d ν d τ = β k Δ t + ∫ t k t k + 1 ( t k + 1 − τ ) η u ( τ ) d τ [see Eq. eg.3.3.x2] (eg.3.3.x1) \begin{aligned}
\int_{t_k}^{t_{k+1}} \textcolor{red}{\beta(\tau)} d\tau &=
\int_{t_k}^{t_{k+1}} \left[ \textcolor{red}{\beta_k + \int_{t_k}^{\tau} \eta_u(\nu) d\nu} \right] d\tau \\
&= \int_{t_k}^{t_{k+1}} \textcolor{red}{\beta_k} d\tau + \int_{t_k}^{t_{k+1}} \textcolor{red}{\int_{t_k}^{\tau} \eta_u(\tau) d\tau} d\tau \\
&= \textcolor{red}{\beta_k} \Delta t + \textcolor{magenta}{\int_{t_k}^{t_{k+1}} \int_{t_k}^{\tau} \eta_u(\nu) d\nu d\tau }\\
&= \beta_k \Delta t + \textcolor{magenta}{\int_{t_k}^{t_{k+1}} (t_{k+1} - \tau) \eta_u(\tau) d\tau } \quad \text{[see Eq. eg.3.3.x2]}\\
\end{aligned}
\tag{eg.3.3.x1}
∫ t k t k + 1 β ( τ ) d τ = ∫ t k t k + 1 [ β k + ∫ t k τ η u ( ν ) d ν ] d τ = ∫ t k t k + 1 β k d τ + ∫ t k t k + 1 ∫ t k τ η u ( τ ) d τ d τ = β k Δ t + ∫ t k t k + 1 ∫ t k τ η u ( ν ) d ν d τ = β k Δ t + ∫ t k t k + 1 ( t k + 1 − τ ) η u ( τ ) d τ [see Eq. eg.3.3.x2] ( e g . 3 . 3 . x 1 )
∫ t k t k + 1 [ ∫ t k τ η u ( ν ) d ν ] d τ = ∫ t k t k + 1 [ [ τ ∫ t k τ η u ( ν ) d ν ] ′ − τ η u ( τ ) ] d τ [see Eq. eg.3.3.x3] = ∫ t k t k + 1 [ τ ∫ t k τ η u ( ν ) d ν ] ′ d τ − ∫ t k t k + 1 τ η u ( τ ) d τ = t k + 1 ∫ t k t k + 1 η u ( ν ) d ν − t k ∫ t k t k η u ( ν ) d ν − ∫ t k t k + 1 τ η u ( τ ) d τ = ∫ t k t k + 1 ( t k + 1 − τ ) η u ( τ ) d τ (eg.3.3.x2) \begin{aligned}
\textcolor{magenta}{\int_{t_k}^{t_{k+1}} \left[ \int_{t_k}^{\tau} \eta_u(\nu) d\nu \right] d\tau}
&= \int_{t_k}^{t_{k+1}} \left[ \left[ \tau \int_{t_k}^\tau \eta_u(\nu)d\nu \right]' - \tau \eta_u(\tau) \right] d\tau \quad \text{[see Eq. eg.3.3.x3]}\\
&= \int_{t_k}^{t_{k+1}} \left[ \tau \int_{t_k}^\tau \eta_u(\nu)d\nu \right]' d\tau - \int_{t_k}^{t_{k+1}} \tau \eta_u(\tau) d\tau \\
&= t_{k+1} \int_{t_k}^{t_{k+1}} \eta_u(\nu)d\nu - \cancel{t_k \int_{t_k}^{t_k} \eta_u(\nu)d\nu} - \int_{t_k}^{t_{k+1}} \tau \eta_u(\tau) d\tau \\
&= \textcolor{magenta}{\int_{t_k}^{t_{k+1}} (t_{k+1} - \tau) \eta_u(\tau) d\tau}
\end{aligned}
\tag{eg.3.3.x2}
∫ t k t k + 1 [ ∫ t k τ η u ( ν ) d ν ] d τ = ∫ t k t k + 1 [ [ τ ∫ t k τ η u ( ν ) d ν ] ′ − τ η u ( τ ) ] d τ [see Eq. eg.3.3.x3] = ∫ t k t k + 1 [ τ ∫ t k τ η u ( ν ) d ν ] ′ d τ − ∫ t k t k + 1 τ η u ( τ ) d τ = t k + 1 ∫ t k t k + 1 η u ( ν ) d ν − t k ∫ t k t k η u ( ν ) d ν − ∫ t k t k + 1 τ η u ( τ ) d τ = ∫ t k t k + 1 ( t k + 1 − τ ) η u ( τ ) d τ ( e g . 3 . 3 . x 2 )
[ τ ∫ t k τ η u ( ν ) d ν ] ′ = ∫ t k τ η u ( ν ) d ν + τ η u ( τ ) [integration by parts] (eg.3.3.x3) \left[ \tau \int_{t_k}^\tau \eta_u(\nu)d\nu \right]'
= \textcolor{red}{\int_{t_k}^\tau \eta_u(\nu)d\nu} + \tau \eta_u(\tau)
\quad \text{[integration by parts]}
\tag{eg.3.3.x3}
[ τ ∫ t k τ η u ( ν ) d ν ] ′ = ∫ t k τ η u ( ν ) d ν + τ η u ( τ ) [integration by parts] ( e g . 3 . 3 . x 3 )
[Keep for records. Only after using the following series derivation, I realized I should have used integration by parts .]
Assuming { τ 1 , τ 2 , ⋯ , τ n } \{ \tau_1, \tau_2, \cdots, \tau_n \} { τ 1 , τ 2 , ⋯ , τ n } , where τ 1 = t k \tau_1 = t_k τ 1 = t k and τ n = t k + 1 \tau_n = t_{k+1} τ n = t k + 1 , and ν i = τ i \nu_i=\tau_i ν i = τ i .
∫ t k t k + 1 ∫ t k τ η u ( ν ) d ν d τ = lim n → ∞ ∑ i = 1 n ∑ j = 1 i η u ( ν j ) δ ν δ τ = lim n → ∞ { η u ( ν 1 ) = + η u ( ν 1 ) + η u ( ν 2 ) = + η u ( ν 1 ) + η u ( ν 2 ) + η u ( ν 3 ) = + ⋯ = + η u ( ν 1 ) + η u ( ν 2 ) + η u ( ν 3 ) + ⋯ + η u ( ν n ) } ⋅ δ ν δ τ = lim n → ∞ ∑ i = 1 n ( n − i + 1 ) η u ( ν i ) δ ν δ τ = lim n → ∞ ∑ i = 1 n ( n − i + 1 ) δ τ η u ( ν i ) δ ν = lim n → ∞ ∑ i = 1 n ( t k + 1 − τ i ) η u ( ν i ) δ ν = ∫ t k t k + 1 ( t k + 1 − τ ) η u ( τ ) d τ \begin{aligned}
\int_{t_k}^{t_{k+1}} \int_{t_k}^{\tau} \eta_u(\nu) d\nu d\tau
&= \lim_{n\rightarrow\infin} \sum_{i = 1}^{n} \sum_{j=1}^{i} \eta_u(\nu_j) \delta\nu \delta\tau \\
&= \lim_{n\rightarrow\infin} \{ \eta_u(\nu_1) \\
&\phantom{=}+ \eta_u(\nu_1) + \eta_u(\nu_2) \\
&\phantom{=}+ \eta_u(\nu_1) + \eta_u(\nu_2) + \eta_u(\nu_3) \\
&\phantom{=}+ \cdots \\
&\phantom{=}+ \eta_u(\nu_1) + \eta_u(\nu_2) + \eta_u(\nu_3) + \cdots + \eta_u(\nu_n) \} \cdot \delta\nu \delta\tau \\
&= \lim_{n\rightarrow\infin} \sum_{i=1}^{n} (n-i+1) \eta_u(\nu_i) \delta\nu \delta\tau \\
&= \lim_{n\rightarrow\infin} \sum_{i=1}^{n} (n-i+1)\delta\tau \eta_u(\nu_i)\delta\nu \\
&= \lim_{n\rightarrow\infin} \sum_{i=1}^{n} (t_{k+1}-\tau_i) \eta_u(\nu_i)\delta\nu \\
&= \int_{t_k}^{t_{k+1}} (t_{k+1}-\tau) \eta_u(\tau) d\tau
\end{aligned}
∫ t k t k + 1 ∫ t k τ η u ( ν ) d ν d τ = n → ∞ lim i = 1 ∑ n j = 1 ∑ i η u ( ν j ) δ ν δ τ = n → ∞ lim { η u ( ν 1 ) = + η u ( ν 1 ) + η u ( ν 2 ) = + η u ( ν 1 ) + η u ( ν 2 ) + η u ( ν 3 ) = + ⋯ = + η u ( ν 1 ) + η u ( ν 2 ) + η u ( ν 3 ) + ⋯ + η u ( ν n ) } ⋅ δ ν δ τ = n → ∞ lim i = 1 ∑ n ( n − i + 1 ) η u ( ν i ) δ ν δ τ = n → ∞ lim i = 1 ∑ n ( n − i + 1 ) δ τ η u ( ν i ) δ ν = n → ∞ lim i = 1 ∑ n ( t k + 1 − τ i ) η u ( ν i ) δ ν = ∫ t k t k + 1 ( t k + 1 − τ ) η u ( τ ) d τ
For the estimation system :
Continuous system:
θ ^ ˙ = ω ~ − β ^ β ^ ˙ = 0 \begin{aligned}
\dot{\hat{\theta}} &= \tilde{\omega} - \hat{\beta} \\
\dot{\hat{\beta}} &= 0
\end{aligned}
θ ^ ˙ β ^ ˙ = ω ~ − β ^ = 0
Discrete system:
θ ^ k + 1 − θ ^ k = ∫ t k t k + 1 θ ^ ˙ ( τ ) d τ = ∫ t k t k + 1 [ ω ~ ( τ ) − β ^ ( τ ) ] d τ = ∫ t k t k + 1 ω ~ ( τ ) d τ − β ^ k Δ t β ^ k + 1 − β ^ k = 0 \begin{aligned}
\hat{\theta}_{k+1} - \hat{\theta}_k
&= \int_{t_k}^{t_{k+1}} \dot{\hat{\theta}}(\tau) d\tau \\
&= \int_{t_k}^{t_{k+1}} [\tilde{\omega}(\tau) - \hat{\beta}(\tau)] d\tau \\
&= \int_{t_k}^{t_{k+1}} \tilde{\omega}(\tau) d\tau - \hat{\beta}_k \Delta t
\\
\hat{\beta}_{k+1} - \hat{\beta}_k &= 0\\
\end{aligned}
θ ^ k + 1 − θ ^ k β ^ k + 1 − β ^ k = ∫ t k t k + 1 θ ^ ˙ ( τ ) d τ = ∫ t k t k + 1 [ ω ~ ( τ ) − β ^ ( τ ) ] d τ = ∫ t k t k + 1 ω ~ ( τ ) d τ − β ^ k Δ t = 0
Then, we have the final discrete-time error propagation equation
θ k + 1 − θ ^ k + 1 = θ k − θ ^ k = + { ∫ t k t k + 1 ω ~ d τ − β k Δ t − ∫ t k t k + 1 [ η v ( τ ) + ( t k + 1 − τ ) η u ( τ ) ] d τ } = − { ∫ t k t k + 1 ω ~ d τ − β ^ k Δ t } = θ k − θ ^ k − ( β k − β ^ k ) Δ t + ∫ t k t k + 1 [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] d τ ≡ θ k − θ ^ k − ( β k − β ^ k ) Δ t + p k β k + 1 − β ^ k + 1 = β k − β ^ k + ∫ t k t k + 1 η u d τ ≡ β k − β ^ k + q k \begin{aligned}
\theta_{k+1} - \hat{\theta}_{k+1} &= \theta_k - \hat{\theta}_k \\
&\phantom{=} + \left\{ \cancel{\int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau}
- \beta_k \Delta t - \int_{t_k}^{t_{k+1}} \left[ \eta_v(\tau) + (t_{k+1} - \tau)\eta_u(\tau) \right] d\tau \right\}\\
&\phantom{=} - \left\{ \cancel{\int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau} - \hat{\beta}_k \Delta t \right\} \\
&= \theta_k - \hat{\theta}_k
- (\beta_k - \hat{\beta}_k) \Delta t
+ \textcolor{magenta}{\int_{t_k}^{t_{k+1}} \left[ - \eta_v(\tau) - (t_{k+1} - \tau)\eta_u(\tau) \right] d\tau} \\
&\equiv \theta_k - \hat{\theta}_k
- (\beta_k - \hat{\beta}_k) \Delta t + \textcolor{magenta}{p_k}
\\
\beta_{k+1} - \hat{\beta}_{k+1} &= \beta_{k} - \hat{\beta}_{k} + \textcolor{magenta}{\int_{t_k}^{t_{k+1}} \eta_u d\tau} \\
&\equiv \beta_{k} - \hat{\beta}_{k} + \textcolor{magenta}{q_k}
\end{aligned}
θ k + 1 − θ ^ k + 1 β k + 1 − β ^ k + 1 = θ k − θ ^ k = + { ∫ t k t k + 1 ω ~ d τ − β k Δ t − ∫ t k t k + 1 [ η v ( τ ) + ( t k + 1 − τ ) η u ( τ ) ] d τ } = − { ∫ t k t k + 1 ω ~ d τ − β ^ k Δ t } = θ k − θ ^ k − ( β k − β ^ k ) Δ t + ∫ t k t k + 1 [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] d τ ≡ θ k − θ ^ k − ( β k − β ^ k ) Δ t + p k = β k − β ^ k + ∫ t k t k + 1 η u d τ ≡ β k − β ^ k + q k
The discrete process noise is
Q = [ Q 11 Q 12 Q 21 Q 22 ] ≡ [ E { p k 2 } E { p k q k } E { q k p k } E { q k 2 } ] \newcommand{\E}[1]{E\left\{ #1\right\}}
Q = \begin{bmatrix} Q_{11} & Q_{12} \\ Q_{21} & Q_{22} \end{bmatrix}
\equiv \begin{bmatrix} \E{p_k^2} & \E{p_kq_k} \\ \E{q_kp_k} & \E{q_k^2} \end{bmatrix}
Q = [ Q 1 1 Q 2 1 Q 1 2 Q 2 2 ] ≡ [ E { p k 2 } E { q k p k } E { p k q k } E { q k 2 } ]
E { p k 2 } = E { ( ∫ t k t k + 1 [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] d τ ) 2 } = E { ∫ t k t k + 1 ∫ t k t k + 1 [ − η v ( ν ) − ( t k + 1 − ν ) η u ( ν ) ] [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] d ν d τ } − − − − − − − − (Paused) (Expand the red integrand) [ η v ( ν ) + ( t k + 1 − ν ) η u ( ν ) ] [ η v ( τ ) + ( t k + 1 − τ ) η u ( τ ) ] = [ η v ( ν ) + ( t k + 1 − ν ) η u ( ν ) ] η v ( τ ) + [ η v ( ν ) + ( t k + 1 − ν ) η u ( ν ) ] ( t k + 1 − τ ) η u ( τ ) = η v ( ν ) η v ( τ ) + ( t k + 1 − ν ) η v ( τ ) η u ( ν ) + ( t k + 1 − τ ) η v ( ν ) η u ( τ ) + ( t k + 1 − ν ) ( t k + 1 − τ ) η u ( ν ) η u ( τ ) − − − − − − − − (Integrate each expanded integrand) ∫ t k t k + 1 η v ( ν ) η v ( τ ) d ν = η v 2 ( τ ) ∫ t k t k + 1 ( t k + 1 − ν ) η v ( τ ) η u ( ν ) d ν = ( t k + 1 − τ ) η v ( τ ) η u ( τ ) = 0 [ σ v and σ u uncorrelated? ] ∫ t k t k + 1 ( t k + 1 − τ ) η v ( ν ) η u ( τ ) d ν = ( t k + 1 − τ ) η v ( τ ) η u ( τ ) = 0 [ σ v and σ u uncorrelated? ] ∫ t k t k + 1 ( t k + 1 − ν ) ( t k + 1 − τ ) η u ( ν ) η u ( τ ) d ν = ( t k + 1 − τ ) 2 ν u 2 ( τ ) − − − − − − − − (continued) = ∫ t k t k + 1 E { η v 2 ( τ ) } d τ + ∫ t k t k + 1 ( t k + 1 − τ ) 2 E { η u 2 ( τ ) } d τ = ∫ t k t k + 1 E { η v 2 ( τ ) } d τ + σ u 2 ∫ t k t k + 1 ( t k + 1 − τ ) 2 d τ = σ v 2 Δ t + σ u 2 − ( t k + 1 − τ ) 3 3 ∣ t k t k + 1 = σ v 2 Δ t + σ u 2 Δ t 3 3 \newcommand{\E}[1]{E\left\{ #1\right\}}
\newcommand{\I}{\int_{t_k}^{t_{k+1}}}
\begin{aligned}
\E{p_k^2}
&= \E{ \left( \I \left[ - \eta_v(\tau) - (t_{k+1} - \tau)\eta_u(\tau) \right] d\tau \right)^2 } \\
&= \E{ \I \textcolor{red}{\I \left[ - \eta_v(\nu) - (t_{k+1} - \nu)\eta_u(\nu) \right] \left[ - \eta_v(\tau) - (t_{k+1} - \tau)\eta_u(\tau) \right] d\nu} d\tau } \\
&-------- \text{(Paused) (Expand the red integrand)} \\
& \left[ \eta_v(\nu) +(t_{k+1} - \nu)\eta_u(\nu) \right] \left[ \eta_v(\tau) + (t_{k+1} - \tau)\eta_u(\tau) \right] \\
&= \left[ \eta_v(\nu) +(t_{k+1} - \nu)\eta_u(\nu) \right] \eta_v(\tau) + \left[ \eta_v(\nu) +(t_{k+1} - \nu)\eta_u(\nu) \right] (t_{k+1} - \tau)\eta_u(\tau)\\
&= \eta_v(\nu)\eta_v(\tau)
+ (t_{k+1} - \nu)\eta_v(\tau)\eta_u(\nu)
+ (t_{k+1} - \tau)\eta_v(\nu)\eta_u(\tau)
+ (t_{k+1} - \nu)(t_{k+1} - \tau)\eta_u(\nu)\eta_u(\tau) \\
&-------- \text{(Integrate each expanded integrand)} \\
&\I \eta_v(\nu)\eta_v(\tau) d\nu = \eta_v^2(\tau)\\
&\I (t_{k+1} - \nu)\eta_v(\tau)\eta_u(\nu) d\nu = (t_{k+1} - \tau)\eta_v(\tau)\eta_u(\tau) = 0 \quad[\sigma_v \text{ and } \sigma_u \text{ uncorrelated?}] \\
&\I (t_{k+1} - \tau)\eta_v(\nu)\eta_u(\tau) d\nu = (t_{k+1} - \tau)\eta_v(\tau)\eta_u(\tau) = 0 \quad[\sigma_v \text{ and } \sigma_u \text{ uncorrelated?}]\\
&\I (t_{k+1} - \nu)(t_{k+1} - \tau)\eta_u(\nu)\eta_u(\tau) d\nu = (t_{k+1} - \tau)^2\nu_u^2(\tau) \\
&-------- \text{(continued)}\\
&= \I \E{ \eta_v^2(\tau) } d\tau + \I (t_{k+1}-\tau)^2\E{ \eta_u^2(\tau) } d\tau \\
&= \I \E{ \eta_v^2(\tau) } d\tau + \sigma_u^2 \I (t_{k+1}-\tau)^2 d\tau \\
&= \sigma_v^2\Delta t + \sigma_u^2 \left. \frac{-(t_{k+1}-\tau)^3}{3} \right|_{t_k}^{t_{k+1}} \\
&= \sigma_v^2\Delta t + \sigma_u^2\frac{\Delta t^3}{3} \\
\end{aligned}
E { p k 2 } = E { ( ∫ t k t k + 1 [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] d τ ) 2 } = E { ∫ t k t k + 1 ∫ t k t k + 1 [ − η v ( ν ) − ( t k + 1 − ν ) η u ( ν ) ] [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] d ν d τ } − − − − − − − − (Paused) (Expand the red integrand) [ η v ( ν ) + ( t k + 1 − ν ) η u ( ν ) ] [ η v ( τ ) + ( t k + 1 − τ ) η u ( τ ) ] = [ η v ( ν ) + ( t k + 1 − ν ) η u ( ν ) ] η v ( τ ) + [ η v ( ν ) + ( t k + 1 − ν ) η u ( ν ) ] ( t k + 1 − τ ) η u ( τ ) = η v ( ν ) η v ( τ ) + ( t k + 1 − ν ) η v ( τ ) η u ( ν ) + ( t k + 1 − τ ) η v ( ν ) η u ( τ ) + ( t k + 1 − ν ) ( t k + 1 − τ ) η u ( ν ) η u ( τ ) − − − − − − − − (Integrate each expanded integrand) ∫ t k t k + 1 η v ( ν ) η v ( τ ) d ν = η v 2 ( τ ) ∫ t k t k + 1 ( t k + 1 − ν ) η v ( τ ) η u ( ν ) d ν = ( t k + 1 − τ ) η v ( τ ) η u ( τ ) = 0 [ σ v and σ u uncorrelated? ] ∫ t k t k + 1 ( t k + 1 − τ ) η v ( ν ) η u ( τ ) d ν = ( t k + 1 − τ ) η v ( τ ) η u ( τ ) = 0 [ σ v and σ u uncorrelated? ] ∫ t k t k + 1 ( t k + 1 − ν ) ( t k + 1 − τ ) η u ( ν ) η u ( τ ) d ν = ( t k + 1 − τ ) 2 ν u 2 ( τ ) − − − − − − − − (continued) = ∫ t k t k + 1 E { η v 2 ( τ ) } d τ + ∫ t k t k + 1 ( t k + 1 − τ ) 2 E { η u 2 ( τ ) } d τ = ∫ t k t k + 1 E { η v 2 ( τ ) } d τ + σ u 2 ∫ t k t k + 1 ( t k + 1 − τ ) 2 d τ = σ v 2 Δ t + σ u 2 3 − ( t k + 1 − τ ) 3 ∣ ∣ ∣ ∣ ∣ t k t k + 1 = σ v 2 Δ t + σ u 2 3 Δ t 3
E { p k q k } = E { ( ∫ t k t k + 1 [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] d τ ) ( ∫ t k t k + 1 η u d τ ) } = E { ∫ t k t k + 1 ∫ t k t k + 1 [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] η u ( ν ) d ν d τ } = − E { ∫ t k t k + 1 ∫ t k t k + 1 [ η v ( τ ) η u ( ν ) + ( t k + 1 − τ ) η u ( τ ) η u ( ν ) ] d ν d τ } = − E { ∫ t k t k + 1 η v ( τ ) η u ( τ ) d τ + ∫ t k t k + 1 ( t k + 1 − τ ) η u 2 ( τ ) d τ } = 0 − ∫ t k t k + 1 ( t k + 1 − τ ) E { η u 2 ( τ ) } d τ = σ u 2 ( t k + 1 − τ ) 2 2 ∣ t k t k + 1 = − σ u 2 Δ t 2 2 \newcommand{\E}[1]{E\left\{ #1\right\}}
\newcommand{\I}{\int_{t_k}^{t_{k+1}}}
\begin{aligned}
\E{p_k q_k}
&= \E{ \left( \I \left[ - \eta_v(\tau) - (t_{k+1} - \tau)\eta_u(\tau) \right] d\tau \right) \left( \int_{t_k}^{t_{k+1}} \eta_u d\tau \right) } \\
&= \E{ \I \I \left[ - \eta_v(\tau) - (t_{k+1} - \tau)\eta_u(\tau) \right] \eta_u(\nu) d\nu d\tau } \\
&= - \E{ \I \I \left[\eta_v(\tau)\eta_u(\nu) + (t_{k+1} - \tau)\eta_u(\tau)\eta_u(\nu) \right] d\nu d\tau } \\
&= -\E{ \I \eta_v(\tau)\eta_u(\tau) d\tau + \I (t_{k+1} - \tau)\eta_u^2(\tau) d\tau } \\
&= 0 - \I (t_{k+1} - \tau)\E{\eta_u^2(\tau)} d\tau \\
&= \sigma_u^2 \left. \frac{(t_{k+1} - \tau)^2}{2} \right|_{t_k}^{t_{k+1}}
= - \sigma_u^2 \frac{\Delta t^2}{2}
\end{aligned}
E { p k q k } = E { ( ∫ t k t k + 1 [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] d τ ) ( ∫ t k t k + 1 η u d τ ) } = E { ∫ t k t k + 1 ∫ t k t k + 1 [ − η v ( τ ) − ( t k + 1 − τ ) η u ( τ ) ] η u ( ν ) d ν d τ } = − E { ∫ t k t k + 1 ∫ t k t k + 1 [ η v ( τ ) η u ( ν ) + ( t k + 1 − τ ) η u ( τ ) η u ( ν ) ] d ν d τ } = − E { ∫ t k t k + 1 η v ( τ ) η u ( τ ) d τ + ∫ t k t k + 1 ( t k + 1 − τ ) η u 2 ( τ ) d τ } = 0 − ∫ t k t k + 1 ( t k + 1 − τ ) E { η u 2 ( τ ) } d τ = σ u 2 2 ( t k + 1 − τ ) 2 ∣ ∣ ∣ ∣ ∣ t k t k + 1 = − σ u 2 2 Δ t 2
E { q k 2 } = E { ( ∫ t k t k + 1 η u d τ ) 2 } = ∫ t k t k + 1 E { η u 2 } d τ = σ u 2 Δ t \newcommand{\E}[1]{E\left\{ #1\right\}}
\newcommand{\I}{\int_{t_k}^{t_{k+1}}}
\begin{aligned}
\E{q_k^2} &= \E{ \left( \int_{t_k}^{t_{k+1}} \eta_u d\tau \right)^2 } \\
&= \I \E{\eta_u^2} d\tau = \sigma_u^2 \Delta t
\end{aligned}
E { q k 2 } = E { ( ∫ t k t k + 1 η u d τ ) 2 } = ∫ t k t k + 1 E { η u 2 } d τ = σ u 2 Δ t
MY-QUESTION-1: If bias is not estimated (Not in the book, double check this!)
For the real system :
Continuous system:
θ ˙ = ω ~ − β ML − η v \begin{aligned}
\dot{\theta} &= \tilde{\omega} - \beta_\text{ML} - \eta_v \\
\end{aligned}
θ ˙ = ω ~ − β ML − η v
Discrete system:
Let Δ t = t k + 1 − t k \Delta t = t_{k+1} - t_{k} Δ t = t k + 1 − t k .
θ k + 1 − θ k = ∫ t k t k + 1 [ ω ~ − β ML − η v ] d τ = ∫ t k t k + 1 ω ~ d τ − ∫ t k t k + 1 β ML ( τ ) d τ − ∫ t k t k + 1 η v d τ = ∫ t k t k + 1 ω ~ d τ − β ML , k Δ t − ∫ t k t k + 1 η v d τ \begin{aligned}
\theta_{k+1} - \theta_{k} &= \int_{t_k}^{t_{k+1}} [\tilde{\omega} - \beta_\text{ML} - \eta_v] d\tau \\
&= \int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau - \int_{t_k}^{t_{k+1}} \beta_\text{ML}(\tau) d\tau - \int_{t_k}^{t_{k+1}} \eta_v d\tau\\
&= \int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau
- \beta_{\text{ML},k} \Delta t
- \int_{t_k}^{t_{k+1}} \eta_v d\tau
\end{aligned}
θ k + 1 − θ k = ∫ t k t k + 1 [ ω ~ − β ML − η v ] d τ = ∫ t k t k + 1 ω ~ d τ − ∫ t k t k + 1 β ML ( τ ) d τ − ∫ t k t k + 1 η v d τ = ∫ t k t k + 1 ω ~ d τ − β ML , k Δ t − ∫ t k t k + 1 η v d τ
For the estimation system :
Continuous system:
θ ^ ˙ = ω ~ − β ML \dot{\hat{\theta}} = \tilde{\omega} - \beta_\text{ML}
θ ^ ˙ = ω ~ − β ML
Discrete system:
θ ^ k + 1 − θ ^ k = ∫ t k t k + 1 θ ^ ˙ ( τ ) d τ = ∫ t k t k + 1 [ ω ~ ( τ ) − β ML ( τ ) ] d τ = ∫ t k t k + 1 ω ~ ( τ ) d τ − β ML , k Δ t \begin{aligned}
\hat{\theta}_{k+1} - \hat{\theta}_k
&= \int_{t_k}^{t_{k+1}} \dot{\hat{\theta}}(\tau) d\tau \\
&= \int_{t_k}^{t_{k+1}} \left[ \tilde{\omega}(\tau) - \beta_\text{ML}(\tau) \right] d\tau \\
&= \int_{t_k}^{t_{k+1}} \tilde{\omega}(\tau) d\tau - \beta_{\text{ML},k} \Delta t
\end{aligned}
θ ^ k + 1 − θ ^ k = ∫ t k t k + 1 θ ^ ˙ ( τ ) d τ = ∫ t k t k + 1 [ ω ~ ( τ ) − β ML ( τ ) ] d τ = ∫ t k t k + 1 ω ~ ( τ ) d τ − β ML , k Δ t
Then, we have the final discrete-time error propagation equation
θ k + 1 − θ ^ k + 1 = θ k − θ ^ k = + { ∫ t k t k + 1 ω ~ d τ − β ML , k Δ t − ∫ t k t k + 1 η v ( τ ) d τ } = − { ∫ t k t k + 1 ω ~ d τ − β ML , k Δ t } = θ k − θ ^ k + ∫ t k t k + 1 − η v ( τ ) d τ ≡ θ k − θ ^ k + p k \begin{aligned}
\theta_{k+1} - \hat{\theta}_{k+1} &= \theta_k - \hat{\theta}_k \\
&\phantom{=} + \left\{ \cancel{\int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau}
- \bcancel{\beta_{\text{ML},k} \Delta t} - \int_{t_k}^{t_{k+1}} \eta_v(\tau) d\tau \right\}\\
&\phantom{=} - \left\{ \cancel{\int_{t_k}^{t_{k+1}} \tilde{\omega} d\tau} - \bcancel{\beta_{\text{ML},k} \Delta t} \right\} \\
&= \theta_k - \hat{\theta}_k
+ \textcolor{magenta}{\int_{t_k}^{t_{k+1}} - \eta_v(\tau) d\tau} \\
&\equiv \theta_k - \hat{\theta}_k
+ \textcolor{magenta}{p_k}
\end{aligned}
θ k + 1 − θ ^ k + 1 = θ k − θ ^ k = + { ∫ t k t k + 1 ω ~ d τ − β ML , k Δ t − ∫ t k t k + 1 η v ( τ ) d τ } = − { ∫ t k t k + 1 ω ~ d τ − β ML , k Δ t } = θ k − θ ^ k + ∫ t k t k + 1 − η v ( τ ) d τ ≡ θ k − θ ^ k + p k
E { p k 2 } = E { ( ∫ t k t k + 1 − η v ( τ ) d τ ) 2 } = E { ∫ t k t k + 1 ∫ t k t k + 1 η v ( ν ) η v ( τ ) d ν d τ } = E { ∫ t k t k + 1 η v 2 ( τ ) d τ } = ∫ t k t k + 1 E { η v 2 ( τ ) } d τ = σ v 2 Δ t \newcommand{\E}[1]{E\left\{ #1\right\}}
\newcommand{\I}{\int_{t_k}^{t_{k+1}}}
\begin{aligned}
\E{p_k^2}
&= \E{ \left( \I - \eta_v(\tau) d\tau \right)^2 } \\
&= \E{ \I \textcolor{red}{\I \eta_v(\nu) \eta_v(\tau) d\nu} d\tau } \\
&= \E{ \I \textcolor{red}{\eta_v^2(\tau)} d\tau } \\
&= \I \E{ \eta_v^2(\tau) } d\tau \\
&= \sigma_v^2\Delta t
\end{aligned}
E { p k 2 } = E { ( ∫ t k t k + 1 − η v ( τ ) d τ ) 2 } = E { ∫ t k t k + 1 ∫ t k t k + 1 η v ( ν ) η v ( τ ) d ν d τ } = E { ∫ t k t k + 1 η v 2 ( τ ) d τ } = ∫ t k t k + 1 E { η v 2 ( τ ) } d τ = σ v 2 Δ t
MY-QUESTION-2: Other process noise?
The above derivations only considered the process noise due to
the difference between the real system model
and the estimation system model
;
the time discretization.
So, it doesn’t consider the process noise due to other differences to the reality
(not the real system model
). If so, do we need an additional term to compensate for additional uncertainties?
(Is my speculation correct?) Maybe not, because the real system model
is also a model of the reality
.
Can I just simply increase σ v \sigma_v σ v and σ u \sigma_u σ u to reflect other model errors?
Because the real system model
is an approximation of the reality
and the last term η v \bm{\eta}_v η v should include or absorb all the differences to reality
.
Similar for σ u \sigma_u σ u .
(Is this speculation correct?)
3.4. The Continuous-Time Kalman Filter
3.5. The Continuous-Discrete Kalman Filter
x ˙ ( t ) = F ( t ) x ( t ) + B ( t ) u ( t ) + G ( t ) w ( t ) y ~ k = H k x k + v k \begin{aligned}
\dot{\bm{x}}(t) &= F(t)\bm{x}(t) + B(t)\bm{u}(t) + G(t)\bm{w}(t)\\
\tilde{\bm{y}}_k &= H_k\bm{x}_k + \bm{v}_k
\end{aligned}
x ˙ ( t ) y ~ k = F ( t ) x ( t ) + B ( t ) u ( t ) + G ( t ) w ( t ) = H k x k + v k
[INSERT TABLE 3.7]
The continuous-time propagation model equation does not involve the measurement directly.
The covariance propagation follows a continuous-time Lyapunov differential equation.
the sample times of the measurements need not occur in regular intervals.
In fact, different measurement sets can be spread out over various time intervals.
The real beauty of the continuous-discreteKalman filter is that it can handle different
scattered measurement sets quite easily.
QUESTIONS:
During the propagation, how is G ( t ) Q ( t ) G ( t ) T G(t)Q(t)G(t)^T G ( t ) Q ( t ) G ( t ) T handled?
This is just a normal term.
G ( t ) w t G(t)\bm{w}_t G ( t ) w t shall be handled as a random term.
How is it handled in Orekit?
3.6. Extended Kalman Filter
The problem with this nonlinear model is that a Gaussian input does not necessarily produce a Gaussian output (unlike the linear case).
The fundamental concept of this filter involves the notion that the true state is sufficiently close to the estimated state.
Therefore, the error dynamics can be represented fairly accurately by a linearized first-order Taylor series expansion.
[INSERT TABLE 3.8]
A steady-state gain cannot be found, because F ( t ) F(t) F ( t ) and H ( t ) H(t) H ( t ) in Table 3.8 will not be constant in general.
Another approach involves linearizing about the normal (a priori) state vector x ˉ ( t ) \bar{\bm{x}}(t) x ˉ ( t ) instead of the current estiamte x ^ ( t ) \hat{\bm{x}}(t) x ^ ( t ) .
This gives the linearized Kalman filter .
In general, the linearized Kalman filter is less accurate than the extended Kalman filter since x ˉ ( t ) \bar{\bm{x}}(t) x ˉ ( t ) is usually not as close to the truth as is x ^ ( t ) \hat{\bm{x}}(t) x ^ ( t ) .
However, since the nominal state is known a priori the gain K ( t ) K(t) K ( t ) can be pre-computed and stored, which reduces the on-line computational burden.
[INSERT TABLE 3.9]
Proving convergence in the extended Kalman filter is difficult (if not impossible!) even for simple systems where the initial condition is not well known.
But it is often robust to initial condition errors, which can often be verified through simulation .
iterated extended Kalman filter :
local iteration at step k k k , until no longer improved.
nonlinear smoother back to time t k − 1 t_{k-1} t k − 1 (see Sec. 5.1.3):
The reference trajectory over [ t k − 1 , t k ) [t_{k-1},t_k) [ t k − 1 , t k ) can also be improved once the measurement y ~ k \tilde{\bm{y}}_k y ~ k is given.
3.7. Unscented Filtering (not needed for now)
3.8. Constrained Filtering (not needed for now)
3.9. Summary
All important formulas are summarized here.
4. Advanced Topics in Sequential State Estimation ()
5. Batch State Estimation (to read later)
Smoothing ??
5.1
5.1.3. (utilized in Sec. 7.4., to read)
6. Parameter Estimation: Applications (to read now)
6.2. Global positioning System Navigation
6.4. Orbit Determination
Gaussian Least Squares Differential Correction (GLSDC)
? ? (6.53) ??
\tag{6.53}
? ? ( 6 . 5 3 )
? ? (6.54) ??
\tag{6.54}
? ? ( 6 . 5 4 )
F ≡ ∂ f ∂ x = [ 0 3 × 3 I 3 × 3 F 21 0 3 × 3 ] (6.57) F \equiv \frac{\partial\bm{f}}{\partial\bm{x}} = \begin{bmatrix}0_{3\times3}&I_{3\times3}\\F_{21}&0_{3\times3}\end{bmatrix} \tag{6.57}
F ≡ ∂ x ∂ f = [ 0 3 × 3 F 2 1 I 3 × 3 0 3 × 3 ] ( 6 . 5 7 )
∂ h ∂ x = [ H 11 0 3 × 3 ] \frac{\partial \bm{h}}{\partial\bm{x}} = \begin{bmatrix}H_{11}&0_{3\times3}\end{bmatrix}
∂ x ∂ h = [ H 1 1 0 3 × 3 ]
7. Estimation of Dynamic Systems: Applications
7.1. Attitude Estimation
q ˙ = 1 2 Ω ( ω ) q (7.1) \dot{\mathbf{q}} = \frac{1}{2} \Omega(\bm{\omega}) \mathbf{q}
\tag{7.1}
q ˙ = 2 1 Ω ( ω ) q ( 7 . 1 )
7.1.2. Discrete-Time Attitude Estimation
The following approach is named “power series approach”, which is used to derive a discrete approximation to the continuous propagation.
Here, the example in the book is for the discrete quaternion propagation of the kinematic equation
q ˙ = 1 2 Ω ( ω ) q (7.1) \dot{\mathbf{q}} = \frac{1}{2} \Omega(\bm{\omega}) \mathbf{q}
\tag{7.1}
q ˙ = 2 1 Ω ( ω ) q ( 7 . 1 )
Ω ( ω ^ ) = [ − [ ω ^ × ] ω ^ − ω ^ T 0 ] = [ 0 − w 3 w 2 w 1 w 3 0 − w 1 w 2 − w 2 w 1 0 w 3 − w 1 − w 2 − w 3 0 ] \newcommand{\ww}[0]{\hat{\bm{\omega}}}
\newcommand{\IwI}[0]{\|\hat{\bm{\omega}}\|}
\newcommand{\OO}[0]{\Omega}
\OO(\ww) = \begin{bmatrix} -[\ww\times] & \ww \\ -\ww^T & 0 \end{bmatrix} = \begin{bmatrix} 0 & -w3 & w2 & w1 \\ w3 & 0 & -w1 & w2 \\ -w2 & w1 & 0 & w3 \\ -w1 & -w2 & -w3 & 0 \end{bmatrix}
Ω ( ω ^ ) = [ − [ ω ^ × ] − ω ^ T ω ^ 0 ] = ⎣ ⎢ ⎢ ⎢ ⎡ 0 w 3 − w 2 − w 1 − w 3 0 w 1 − w 2 w 2 − w 1 0 − w 3 w 1 w 2 w 3 0 ⎦ ⎥ ⎥ ⎥ ⎤
Ω 2 ( ω ^ ) = − ∥ ω ^ ∥ 2 I 4 × 4 Ω 3 ( ω ^ ) = − ∥ ω ^ ∥ 2 Ω ( ω ^ ) ⋯ Ω 2 k ( ω ^ ) = ( − 1 ) k ∥ ω ^ ∥ 2 k I 4 × 4 ( k ≥ 0 , Ω 0 = I 4 × 4 does satisfy. ) Ω 2 k + 1 ( ω ^ ) = ( − 1 ) k ∥ ω ^ ∥ 2 k Ω ( ω ^ ) (7.36) \newcommand{\ww}[0]{\hat{\bm{\omega}}}
\newcommand{\IwI}[0]{\|\hat{\bm{\omega}}\|}
\newcommand{\OO}[0]{\Omega}
\begin{aligned}
\OO^2(\ww) &= -\IwI^2 I_{4\times4} \\
\OO^3(\ww) &= -\IwI^2 \OO(\ww) \\
&\cdots \\
\OO^{2k}(\ww) &= (-1)^k\IwI^{2k} I_{4\times4} \quad(k\geq0, \textcolor{blue}{\OO^0=I_{4\times4} \text{ does satisfy.}})\\
\OO^{2k+1}(\ww) &= (-1)^k\IwI^{2k} \OO(\ww) \\
\end{aligned}
\tag{7.36}
Ω 2 ( ω ^ ) Ω 3 ( ω ^ ) Ω 2 k ( ω ^ ) Ω 2 k + 1 ( ω ^ ) = − ∥ ω ^ ∥ 2 I 4 × 4 = − ∥ ω ^ ∥ 2 Ω ( ω ^ ) ⋯ = ( − 1 ) k ∥ ω ^ ∥ 2 k I 4 × 4 ( k ≥ 0 , Ω 0 = I 4 × 4 does satisfy. ) = ( − 1 ) k ∥ ω ^ ∥ 2 k Ω ( ω ^ ) ( 7 . 3 6 )
exp [ 1 2 Ω ( ω ^ ) t ] = ∑ j = 0 ∞ [ 1 2 Ω ( ω ^ ) t ] j j ! = ∑ k = 0 ∞ [ 1 2 t ] ( 2 k ) ( − 1 ) k ∥ ω ^ ∥ 2 k I 4 × 4 ( 2 k ) ! + ∑ k = 0 ∞ [ 1 2 t ] ( 2 k + 1 ) ( − 1 ) k ∥ ω ^ ∥ 2 k Ω ( ω ^ ) ( 2 k + 1 ) ! = I 4 × 4 ∑ k = 0 ∞ ( − 1 ) k [ 1 2 ∥ ω ^ ∥ t ] ( 2 k ) ( 2 k ) ! + Ω ( ω ^ ) ∥ ω ^ ∥ ∑ k = 0 ∞ ( − 1 ) k [ 1 2 ∥ ω ^ ∥ t ] ( 2 k + 1 ) ( 2 k + 1 ) ! = I 4 × 4 cos ( 1 2 ∥