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

24

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,(xkxk1)/dtHH=[1   0]xkxkx˙k

xk=(xkx˙k)

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órmulakx^kk1xk1x˙k1ak1kx^a

x^k=xk1+x˙k1dt+12adt2

Isto implica que, no tempo , uma medição ˙ x k da velocidade é dado pelakx˙^k

x˙^k=x˙k1+adt=2x^kxk1dtx˙k1

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çãox^kxk1x˙k1R

x^k=(x^kx˙^k)

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

Estocástico
fonte
2
Não examinei todos os seus cálculos. No entanto, falando do exemplo da Wikipedia, você parece estar um pouco confuso em sua estrutura. Você está correto nessa única posição é medida. No entanto, é utilizado o chamado modelo de "velocidade constante". Isso significa que a velocidade é considerada constante na matriz de transição de estado.
Jason R
3
Alterações na velocidade são modeladas usando a matriz de ruído do processo. Portanto, você está assumindo inerentemente que a velocidade mudará aleatoriamente com alguma covariância especificada. Surpreendentemente, isso geralmente funciona bem. É comum usar o ruído do processo, uma derivada acima da sua derivada variável de estado mais alta dessa maneira. Por exemplo, se você incluiu aceleração em seu modelo, poderá ter um componente de empurrão aleatório incluído no ruído do processo.
Jason R
@JasonR com o modelo da Wikipedia (assumindo zero covariância inicial entre posição e velocidade), a estimativa de velocidade é sempre o seu valor inicial (como você diz, um modelo de "velocidade constante"). No entanto, a variação da velocidade aumenta monotonicamente através do ruído do processo, e não há medições que possam reduzi-lo. Qual é a vantagem disso em relação a um modelo que apenas modela a posição e assume uma velocidade constante?
Estocástica
2
A variação da estimativa de velocidade não deve aumentar monotonicamente. O ruído do processo simplesmente introduz um componente estocástico na equação de transição de estado, permitindo que você expresse alguma incerteza sobre exatamente como o estado do sistema evoluirá de um passo para o outro. Se você não incluir o ruído do processo, seu filtro realmente produzirá uma velocidade constante. Provavelmente não é isso que você deseja.
Jason R
Bem, @JasonR, se você olhar para o modelo da Wikipedia, verá que uma variação monotinicamente crescente de velocidade é o que ela oferece!
Estocástica

Respostas:

24

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

xk

xk+1=xk+x˙kΔt+12a(Δt)2x˙k+1=x˙k+aΔt

xk=(xkx˙k)
xk+1=(1  Δt0  1)xk+((Δt)22Δt)ak
ak

GFH


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

yy

Os gráficos seguintes mostram o resultado.

yxk

insira a descrição da imagem aqui

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

insira a descrição da imagem aqui

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

insira a descrição da imagem aqui

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

insira a descrição da imagem aqui

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.

insira a descrição da imagem aqui

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

insira a descrição da imagem aqui

zk+1=xk+1Pk=0Pk+1=QKk+1

Kk+1=(12/dt)

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

x^k+1=Fk+1xk+Kk+1(zk+1Hk+1Fk+1xk)=(xk+x˙kdtx˙k)+(12/dt)(xk+1(xk+x˙kdt))=(xk+12(xk+1xk)/dtx˙k)

(xkxk1)/dt

Peter K.
fonte
Obrigado por toda sua ajuda até agora. Minha pergunta original continha alguns mal-entendidos, então tentei reorientá-la e com uma tentativa de responder sua pergunta sobre z_k.
Estocástica
1
obrigado vm por todos os seus esforços :-). Seu trabalho me incentivou a fazer um pouco de matemática que eu prendi na sua resposta, espero que você não se importe. De qualquer forma, agora estou 100% convencido de que o método padrão é bom, porque, afinal, posso ver fórmulas lá para velocidade. Obrigado novamente #
Stochastically
Fico feliz em poder ajudar! Não há problema em adicionar à resposta.
Peter K.