Como escolho os parâmetros de um filtro Kalman?

Suponha que eu queira rastrear a posição de um carro em 2D. O que eu recebo como dados do sensor é minha posição atual. Então meu estado é

$$\mathbf{x} = \begin{pmatrix}x\\y\\\dot{x}\\\dot{y}\end{pmatrix}$$

Onde $x \in \mathbb{R}$ é a posição em m de distância de algum ponto predefinido, $\dot{x} \in \mathbb{R}$ é a velocidade em m / s no momento inicial e $\ddot{x} \in \mathbb{R}$ é a aceleração em $m/s^2$. As medições são

$$\mathbf{z} = \begin{pmatrix}x^{(M)}\\y^{(M)}\end{pmatrix}$$

O que escolho é a minha aceleração a cada etapa do tempo $i$ (os intervalos de tempo têm a duração $t$):

$$u = \begin{pmatrix}\ddot{x}^{(u)}\\\ddot{y}^{(u)}\end{pmatrix}$$

Como o filtro Kalman é um filtro linear, meu modelo de estado é:

$$\mathbf{x}^{(P)} = A x + Bu$$

A medição depende do estado, com algum ruído $v$:

$$\mathbf{z} = H \mathbf{x} + v$$

com $A \in \mathbb{R}^{4 \times 4}$, $H \in \mathbb{R}^{2 \times 4}$. Como se pode decompor a aceleração / velocidade nas direções e a equação para a nova posição é

$$\begin{align}x_{new}(t) &= x + \dot{x} t + 0.5 \ddot{x} t^2\\ y_{new}(t) &= y + \dot{y} t + 0.5 \ddot{y} t^2\\ \dot{x}_{new}(t) &= \dot{x} + \ddot{x} t\\ \dot{y}_{new}(t) &= \dot{y} + \ddot{y} t\end{align}$$

Portanto, dado o nosso modelo de estado, obtemos:

$$\mathbf{x}^{(P)} = \underbrace{\begin{pmatrix}1& 0 & t & 0\\ 0& 1 & 0 & t\\ 0& 0 & 1 & 0\\ 0& 0 & 0 & 1\end{pmatrix}}_{A_i} \mathbf{x} + \underbrace{\begin{pmatrix}0.5t^2 & 0\\ 0 & 0.5t^2\\ t & 0\\ 0 & t\end{pmatrix}}_{B_i} \cdot u_i$$

1. Até agora, este é um cenário / abordagem razoável para o filtro Kalman?

2. Como escolho a matriz de covariância inicial da incerteza $P_0 \in \mathbb{R}^{4 \times 4}$ / o estado inicial $\mathbf{x}$? Ouvi dizer que a maioria torna os valores da matriz "grandes" - o que isso significa. Por exemplo, deve ser uma matriz diagonal

   $$P_0 = \begin{pmatrix}a_1 & 0 & 0 & 0\\ 0 & a_2 & 0 & 0\\ 0 & 0 & a_3 & 0\\ 0 & 0 & 0 & a_4\end{pmatrix}$$ para alguns $a \in \mathbb{R}^+$? Por exemplo,$a_1 = a_2 = 20000000$ como o diâmetro da terra é de cerca de $40000\textrm{ km}$ e $a_3=a_4=90$ como indo mais do que $324\textrm{ km/h}$ nunca vai acontecer para um carro?

   Para o parâmetro de estado inicial, eu esperaria duas etapas de tempo:

   $$\mathbf{x}_0 = \begin{pmatrix}x^{(M)}_{-1}\\ y^{(M)}_{-1}\\ x^{(M)}_{-1} - x^{(M)}_{-2}\\ y^{(M)}_{-1} - y^{(M)}_{-2}\end{pmatrix}$$

Etapa de previsão

A previsão de estado funciona como acima:

$$\mathbf{x}^{(P)}_{i+1} = A_i \mathbf{x}_{i} + B_i u_i$$

Previsão de covariância:

$$P_{i+1}^{(P)} = A P_i A^T + Q \quad \text{with}\quad Q \in \mathbb{R}^{4 \times 4}. \tag{P}$$

3. Onde obtenho a covariância de erros do processo $Q$de? Quais propriedades ele precisa ter? Eu acho positivo definitivo? O que essa matriz significa?

Etapa de inovação

Inovação, que compara a medida com a previsão:

$$\tilde{y}_{i+1} = z_{i+1} - H \mathbf{x}^{(P)}_{i+1}$$

4. (resolvido) : Onde obtenho a matriz de observação$H \in \mathbb{R}^{2 \times 4}$de? O que isso significa?

EDIT :

Deixa comigo. No meu exemplo

$$H = \begin{pmatrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\end{pmatrix},$$ pois codifica a relação entre o estado e a medida.

Covariância da inovação:

$$S_{i+1} = H P_{i+1}^{(P)} H^T + R$$

Para a covariância do erro de medição $R \in \mathbb{R}^{2 \times 2}$Eu tenho que saber algo sobre o funcionamento dos meus sensores. Eu acho que isso geralmente será uma matriz diagonal, pois os sensores serão independentes (?).

Ganho de Kalman:

$$K_{i+1} = P_{i+1}^{(P)} H^T S^{-1}_{i+1}$$

Agora, finalmente, a atualização de estado e covariância:

$$x_{i+1} = \mathbf{x}^{(P)}_{i+1} + K_{i+1} \tilde{y}$$ $$P_{i+1} = (I - K_{i+1} H) P_{i+1}^{(P)}$$

Fontes:

• http://greg.czerniak.info/guides/kalman1/ (Encontrei isso enquanto escrevia esta pergunta - ela já respondeu algumas das minhas perguntas, que espero removi.)
• Várias palestras no KIT

Até agora, este é um cenário / abordagem razoável para o filtro Kalman?

Resposta 1: Sim, seu modelo parece razoável. Você está tratando a aceleração como constante, no entanto. Se isso mudar na sua experiência, inclua isso na matriz de erros do sistema$Q$.

Como escolho a matriz de covariância inicial da incerteza $P_0 \in \mathbb{R}^{4 \times 4}$ / o estado inicial $\mathbf{x}$?

Resposta 2: $P_0$é a sua covariância inicial do estado . Isso expressa o quanto você sabe sobre a estimativa inicial do estado$\mathbf{x}_0$. Se você não tem ideia, é comum definir

$$P_0 = \sigma^2I_4$$ Onde $\sigma^2$é grande. Veja, por exemplo, esta resposta que define$\sigma^2 = 1000$.

Onde obtenho a covariância de erros do processo $Q$de? Quais propriedades ele precisa ter? Eu acho positivo definitivo? O que essa matriz significa?

Resposta 3: Essa matriz expressa o erro do seu sistema. As entradas nesta matriz representam a covariância dos valores correspondentes no seu modelo de sistema. Se você assume, por exemplo, a aceleração tão constante como você, mas vai mudar no mundo real, convém incluir aqui a covariância correspondente.

Além disso, é sempre uma boa ideia experimentar o seu $Q$ e $P$matrizes muito. Fazer$Q$ maior dependerá mais de seus dados ao vivo, tornando $P$maior dependerá mais de sua previsão. Raramente um modelo é perfeito sem ajuste.