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 ^ )
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 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 + x ~ k + \tilde{\bm{x}}_k^+ x ~ k + x ~ k + 1 − \tilde{\bm{x}}_{k+1}^- x ~ k + 1 − x ~ k \tilde{\bm{x}}_k x ~ k 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
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.
3.3.4. Steady-State Kalman Filter
For time-invariant systems the error covariance P P P K K K 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 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 v k \bm{v}_k v k
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)w k − 1 \bm{w}_{k-1} w k − 1 v k \bm{v}_k v k w k − 1 \bm{w}_{k-1} w k − 1
3.3.7. Cramer-Rao Lower Bound
see wikipedia :
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 } τ 1 = t k \tau_1 = t_k τ 1 = t k τ n = t k + 1 \tau_n = t_{k+1} τ n = t k + 1 ν 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 σ u \sigma_u σ u real system model is an approximation of the reality and the last term η v \bm{\eta}_v η v reality.σ 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 sample times of the measurements need not occur in regular intervals.
The real beauty of the continuous-discreteKalman filter is that it can handle different
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
This is just a normal term.
G ( t ) w t G(t)\bm{w}_t G ( t ) w t
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.
[INSERT TABLE 3.8]
A steady-state gain cannot be found, because F ( t ) F(t) F ( t ) H ( t ) H(t) H ( t )
Another approach involves linearizing about the normal (a priori) state vector x ˉ ( t ) \bar{\bm{x}}(t) x ˉ ( t ) x ^ ( t ) \hat{\bm{x}}(t) x ^ ( t )
This gives the linearized Kalman filter x ˉ ( t ) \bar{\bm{x}}(t) x ˉ ( t ) x ^ ( t ) \hat{\bm{x}}(t) x ^ ( t ) K ( t ) K(t) K ( t )
[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.verified through simulation .
iterated extended Kalman filter k k k
nonlinear smoother t k − 1 t_{k-1} t k − 1 [ t k − 1 , t k ) [t_{k-1},t_k) [ t k − 1 , t k ) y ~ k \tilde{\bm{y}}_k y ~ k
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.
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 ∥