﻿ 协方差外插方程

# 协方差外插方程

$\boldsymbol{P}_{n+1,n} = \boldsymbol{FP}_{n,n}\boldsymbol{F}^{T} + \boldsymbol{Q}$

 $$\boldsymbol{P}_{n,n}$$ 是当前状态估计的不确定性的平方（协方差矩阵） $$\boldsymbol{P}_{n+1,n}$$ 是下一个状态预测的不确定性的平方（协方差矩阵） $$\boldsymbol{F}$$ 是在“线性动态系统建模”一节推导的状态转移矩阵 $$\boldsymbol{Q}$$ 是过程噪声矩阵

## 无过程噪声的估计协方差

$\boldsymbol{P}_{n+1,n} = \boldsymbol{FP}_{n,n}\boldsymbol{F}^{T}$

$COV(\boldsymbol{x}) = E \left( \left( \boldsymbol{x - \mu_{x}} \right) \left( \boldsymbol{x - \mu_{x}} \right)^{T} \right)$

$\boldsymbol{P}_{n,n} = E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right)$

$\boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{G\hat{u}}_{n,n}$

$\boldsymbol{P}_{n+1,n} = E \left( \left( \boldsymbol{\hat{x}}_{n+1,n} - \boldsymbol{\mu}_{x_{n+1,n}} \right) \left( \boldsymbol{\hat{x}}_{n+1,n} - \boldsymbol{\mu}_{x_{n+1,n}} \right)^{T} \right) =$

$= E \left( \left( \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{G\hat{u}}_{n,n} - \boldsymbol{F\mu_{x}}_{n,n} - \boldsymbol{G\hat{u}}_{n,n} \right) \left( \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{G\hat{u}}_{n,n} - \boldsymbol{F\mu_{x}}_{n,n} - \boldsymbol{G\hat{u}}_{n,n} \right)^{T} \right) =$

$= E \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}}_{n,n} - \boldsymbol{\mu}_{x_{n,n}} \right) \left( \boldsymbol{F} \left( \boldsymbol{\hat{x}}_{n,n} - \boldsymbol{\mu}_{x_{n,n}} \right) \right)^{T} \right) =$

$= E \left(\boldsymbol{F} \left( \boldsymbol{\hat{x}}_{n,n} - \boldsymbol{\mu}_{x_{n,n}} \right) \left( \boldsymbol{\hat{x}}_{n,n} - \boldsymbol{\mu}_{x_{n,n}} \right)^{T} \boldsymbol{F}^{T} \right) =$

$= \boldsymbol{F} E \left( \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right) \left( \boldsymbol{\hat{x}_{n,n} - \mu_{x_{n,n}}} \right)^{T} \right) \boldsymbol{F}^{T} =$

$= \boldsymbol{F} \boldsymbol{P}_{n,n} \boldsymbol{F}^{T}$

## 构建过程噪声矩阵 $$Q$$

$\boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{G\hat{u}}_{n,n} + \boldsymbol{w}_{n}$

$\boldsymbol{Q} = \left[ \begin{matrix} q_{11} & 0 & \cdots & 0 \\ 0 & q_{22} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & q_{kk} \\ \end{matrix} \right]$

• 离散噪声模型
• 连续噪声模型

### 离散噪声模型

$\boldsymbol{Q} = \left[ \begin{matrix} V(x) & COV(x,v) \\ COV(v,x) & V(v) \\ \end{matrix} \right]$

$V(v) = \sigma^{2}_{v} = E\left(v^{2}\right) - \mu_{v}^{2} = E \left( \left( a\Delta t\right)^{2}\right) - \left(\mu_{a}\Delta t\right)^{2} = \Delta t^{2}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \Delta t^{2}\sigma^{2}_{a}$

$V(x) = \sigma^{2}_{x} = E\left(x^{2}\right) - \mu_{x}^{2} = E \left( \left( \frac{1}{2}a\Delta t^{2}\right)^{2}\right) - \left(\frac{1}{2}\mu_{a}\Delta t^{2}\right)^{2} = \frac{\Delta t^{4}}{4}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \frac{\Delta t^{4}}{4}\sigma^{2}_{a}$

$COV(x,v) = COV(v,x) = E\left(xv\right) - \mu_{x}\mu_{v} = E\left( \frac{1}{2}a\Delta t^{2}a\Delta t\right) - \left( \frac{1}{2}\mu_{a}\Delta t^{2}\mu_{a}\Delta t\right) = \frac{\Delta t^{3}}{2}\left( E\left(a^{2}\right) - \mu_{a}^{2} \right) = \frac{\Delta t^{3}}{2}\sigma^{2}_{a}$

$\boldsymbol{Q} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right]$

#### 状态转移矩阵投影法

$\boldsymbol{Q}_{a} = \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \sigma^{2}_{a}$

$\boldsymbol{Q} = \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F}^{T}$

$\boldsymbol{F} = \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right]$

$\boldsymbol{Q} = \boldsymbol{F}\boldsymbol{Q}_{a}\boldsymbol{F}^{T} =$

$= \left[ \begin{matrix} 1 & \Delta t & \frac{\Delta t^{2}}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ \Delta t & 1 & 0 \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} =$

$= \left[ \begin{matrix} 0 & 0 & \frac{\Delta t^{2}}{2} \\ 0 & 0 & \Delta t \\ 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} 1 & 0 & 0 \\ \Delta t & 1 & 0 \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a} =$

$= \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} & \frac{\Delta t^{2}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} & \Delta t \\ \frac{\Delta t^{2}}{2} & \Delta t & 1 \\ \end{matrix} \right] \sigma^{2}_{a}$

#### 输入转移矩阵投影法

$\boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}}$

$\boldsymbol{G} = \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right]$

$\boldsymbol{Q} = \boldsymbol{G}\sigma^{2}_{a}\boldsymbol{G^{T}} = \sigma^{2}_{a}\boldsymbol{G}\boldsymbol{G^{T}} = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{2}}{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \frac{\Delta t^{2}}{2} & \Delta t \\ \end{matrix} \right] = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right]$

### 连续噪声模型

$\boldsymbol{Q}_{c} = \int _{0}^{ \Delta t}\boldsymbol{Q}dt = \int _{0}^{ \Delta t} \sigma^{2}_{a} \left[ \begin{matrix} \frac{t^{4}}{4} & \frac{t^{3}}{2} \\ \frac{t^{3}}{2} & t^{2} \\ \end{matrix} \right] dt = \sigma^{2}_{a} \left[ \begin{matrix} \frac{\Delta t^{5}}{20} & \frac{\Delta t^{4}}{8} \\ \frac{\Delta t^{4}}{8} & \frac{\Delta t^{3}}{3} \\ \end{matrix} \right]$