Filtro de Kalman para posição e velocidade: introdução de estimativas de velocidade

Obrigado a todos que postaram comentários / respostas à minha consulta ontem ( Implementando um filtro Kalman para posição, velocidade, aceleração ). Eu estive analisando o que foi recomendado e, em particular, (a) o exemplo da Wikipedia em uma posição dimensional e velocidade e também em outro site que considera algo semelhante .

Atualização 26-abr-2013 : a pergunta original aqui continha alguns erros, relacionados ao fato de eu não ter entendido corretamente o exemplo da Wikipedia em uma posição e velocidade unidimensionais . Com minha compreensão aprimorada do que está acontecendo, agora reformulei a questão e a concentrei mais.

Os dois exemplos a que me refiro no parágrafo introdutório acima assumem que é apenas a posição que é medida. No entanto, nenhum dos exemplos possui nenhum tipo de cálculo para velocidade. Por exemplo, o exemplo da Wikipedia especifica a matriz H como H = [ 1 0 ] , o que significa que apenas a posição é inserida. Focando no exemplo da Wikipedia, o vetor de estado x k do filtro Kalman contém a posição x k e a velocidade ˙ x k , ou seja,$(x_k-x_{k-1})/dt$${\bf H}$${\bf H} = [1\ \ \ 0]$${\bf x}_k$$x_k$$\dot{x}_{k}$

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$

Suponhamos que a medição da posição no momento é x k . Em seguida, se a posição e velocidade no momento k - 1 eram x k - 1 e ˙ x k - 1 , e, se uma é uma aceleração constante que aplica-se no intervalo de tempo k - 1 a k , a partir da medição de x é possível deduzir um valor para a usando a fórmula$k$$\hat{x}_k$$k-1$$x_{k-1}$$\dot{x}_{k-1}$$a$$k-1$$k$$\hat{x}$$a$

$$\hat{x}_k = x_{k-1} + \dot{x}_{k-1} dt + \frac{1}{2} a dt^2$$

Isto implica que, no tempo , uma medição ˙ x k da velocidade é dado pela$k$$\hat{\dot{x}}_k$

$$\hat{\dot{x}}_k = \dot{x}_{k-1} + a dt = 2 \frac{\hat{x}_k - {x}_{k-1}}{dt} - \dot{x}_{k-1}$$

Todas as quantidades do lado direito da equação que x k , x k - 1 e ˙ x k - 1 ) são normalmente distribuídos variáveis aleatórias com meios conhecidos e os desvios padrão, de modo que o R matriz para o vector de medição$\hat{x}_k$$x_{k-1}$$\dot{x}_{k-1}$$\bf R$

$$\begin{align*} \mathbf{\hat{x}}_{k} & =\left(\begin{array}[c]{c}\hat{x}_{k}\\ \hat{\dot{x}}_{k}\end{array} \right) \end{align*}$$

pode ser calculado. Essa é uma maneira válida de introduzir estimativas de velocidade no processo?

Essa é uma maneira válida de introduzir estimativas de velocidade no processo?

Se você escolher seu estado adequadamente, as estimativas de velocidade serão "gratuitas". Veja a derivação do modelo de sinal abaixo (para o caso 1-D simples que estamos vendo).

Modelo de Sinal, Take 2

$x_k$

$$\begin{array} xx_{k+1} &=& x_{k} + \dot{x}_{k} \Delta t + \frac{1}{2} a (\Delta t)^2\\ \dot{x}_{k+1} &=& \dot{x}_{k} + a \Delta t \end{array}$$

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$ $$\mathbf{x}_{k+1} = \left(\begin{array}[c]{c} 1\ \ \Delta t\\ 0\ \ 1\end{array} \right) \mathbf{x}_{k} + \left(\begin{array}[c]{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right) a_k$$ $a_k$

$\mathbf{G}$$\mathbf{F}$$\mathbf{H}$

---

Se eu implementar isso em scilab(desculpe, sem acesso ao matlab), ele se parece com:

// Signal Model
DeltaT = 0.1;
F = [1 DeltaT; 0 1];
G = [DeltaT^2/2; DeltaT];
H = [1 0];

x0 = [0;0];
sigma_a = 0.1;

Q = sigma_a^2;
R = 0.1;

N = 1000;

a = rand(1,N,"normal")*sigma_a;

x_truth(:,1) = x0;
for t=1:N,
    x_truth(:,t+1) = F*x_truth(:,t) + G*a(t);
    y(t) = H*x_truth(:,t) + rand(1,1,"normal")*sqrt(R);
end

$y$

// Kalman Filter
p0 = 100*eye(2,2);

xx(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);
for t=1:N,
    [x1,p1,x,p] = kalm(y(t),xx(:,t),pp,F,G,H,Q,R);
    xx(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

$y$$y$

Os gráficos seguintes mostram o resultado.

$y$$x_k$

Gráfico 2 : uma visão ampliada das primeiras amostras:

Gráfico 3 : Algo que você nunca consegue na vida real, a posição verdadeira versus a estimativa de estado da posição.

Gráfico 4 : Algo que você também nunca obtém na vida real, a velocidade verdadeira versus a estimativa de estado da velocidade.

Gráfico 5 : A norma da matriz de covariância do estado (algo que você deve sempre monitorar na vida real!). Observe que ele passa rapidamente de seu valor inicial muito grande para algo muito pequeno, então eu mostrei apenas as primeiras amostras.

Gráfico 6 : gráficos do erro entre a verdadeira posição e velocidade e suas estimativas.

$\mathbf{z}_{k+1}=x_{k+1}$$\mathbf{P}_k=0$$\mathbf{P}_{k+1}^{-}=\mathbf{Q}$$\mathbf{K}_{k+1}$

$$\mathbf{K}_{k+1} = \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right)$$

Isso significa que o procedimento de atualização do Kalman produz

$$\begin{align*} \mathbf{\hat{x}}_{k+1} & = \mathbf{F}_{k+1}\mathbf{x}_k + \mathbf{K}_{k+1}\left(\mathbf{z}_{k+1} - \mathbf{H}_{k+1} \mathbf{F}_{k+1}\mathbf{x}_k\right)\\ & = \left(\begin{array}[c]{c}x_k + \dot{x}_k dt\\ \dot{x}_k\end{array} \right) + \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right) \left(x_{k+1} - \left( x_k + \dot{x}_k dt\right) \right)\\ & = \left(\begin{array}[c]{c}x_{k+1}\\ 2 \left(x_{k+1} - x_k \right) /dt - \dot{x}_k\end{array} \right) \end{align*}$$

$(x_k-x_{k-1})/dt$