Introducción al concepto de filtro de Kalman (filtro de Kalman) y derivación detallada de la fórmula

filtro Kalman

El filtro de Kalman es un filtro recursivo de alta eficiencia ( filtro autorregresivo ) que puede estimar el estado de un sistema dinámico a partir de una serie de mediciones incompletas y ruidosas . El filtrado de Kalman considerará la distribución conjunta en cada momento según el valor de cada medida en diferentes momentos, y luego generará una estimación de la variable desconocida, por lo que será más preciso que el método de estimación basado en una sola medida. El filtrado de Kalman recibe su nombre de Rudolf Kalman, uno de los principales contribuyentes .

El algoritmo del filtro de Kalman es un procedimiento de dos pasos. En el paso de estimación, el filtro de Kalman produce una estimación sobre el estado actual, que también incluye la incertidumbre. Simplemente observe la siguiente medición (que debe contener algún grado de error, incluido el ruido aleatorio). Las estimaciones se actualizan a través de un promedio ponderado y las mediciones con mayor certeza se ponderan más. El algoritmo es iterativo y se puede ejecutar en un sistema de control en tiempo real , requiriendo solo las mediciones de entrada actuales, los valores calculados en el pasado y su matriz de incertidumbre, y no se requiere ninguna otra información pasada.

Aplicaciones

Un ejemplo típico de filtrado de Kalman es predecir las coordenadas y la velocidad de la posición del objeto a partir de un conjunto limitado de secuencias ruidosas de observaciones de la posición del objeto ( posiblemente sesgadas ) . Se puede encontrar en muchas aplicaciones de ingeniería (como radar , visión artificial ). Al mismo tiempo, el filtrado de Kalman también es un tema importante en la teoría de control y la ingeniería de sistemas de control.

Por ejemplo, con el radar, la gente está interesada en su capacidad para rastrear objetivos. Pero los valores medidos de la posición, la velocidad y la aceleración del objetivo suelen ser ruidosos en cualquier momento. El filtrado de Kalman utiliza la información dinámica del objetivo para tratar de eliminar la influencia del ruido y obtener una buena estimación de la posición del objetivo . Esta estimación puede ser una estimación de la posición de destino actual (filtrado), una estimación de posiciones futuras (predicción) o una estimación de posiciones pasadas (interpolación o suavizado).

Modelo de sistema dinámico básico

El filtrado de Kalman se basa en álgebra lineal y modelos ocultos de Markov (modelo oculto de Markov). Su sistema dinámico básico se puede representar mediante una cadena de Markov construida sobre un operador lineal perturbado por el ruido gaussiano (es decir, ruido distribuido normalmente) . El estado del sistema se puede representar mediante un vector cuyos elementos son números reales . Con cada aumento del tiempo discreto , este operador lineal actuará sobre el estado actual para generar un nuevo estado, y también introducirá algo de ruido y, al mismo tiempo, también se incorporará cierta información de control del controlador conocida del sistema. Al mismo tiempo, otro operador lineal, corrompido por el ruido, produce la salida visible de estos estados ocultos.

Para utilizar el filtro de Kalman para estimar el estado interno del proceso observado a partir de una serie de datos de observación ruidosos, el proceso debe modelarse bajo el marco del filtro de Kalman. Es decir, para cada paso k , defina la matriz $F_{}$, $H_{}$, $q_{}$, $R_{}$, a veces también es necesario definir $B_{}$,como sigue.

El modelo de filtro de Kalman asume $k+El\; estado\; real\; en\; el\; momento\; 1$ es dekkEl estado en el tiempo$k$ evoluciona y se ajusta a la ecuación de estado:
$$X_{}=F_{}{X}_{k-}+B_{}{tu}_{}+w_{}$$
en

• $F_{}$está actuando sobre $X_{k-}$El modelo de transición de estado (matriz o vector) en .
• $B_{}$está actuando sobre el controlador vector ${tu}_{}$El modelo de control de entrada en .
• $w_{}$es el ruido del proceso, y se supone que su media es cero, y la matriz de covarianza es $q_{}$distribución normal multivariante.

​wk$w_{}\sim N(0,q_{})$

donde $cov\left\{w\right(k)\}=Q\left(k\right)$ representa la matriz de covarianza.

​ En el tiempo k, para bienes inmuebles $X_{}$Una medida de $z_{}$Satisfaga la siguiente fórmula, es decir, la ecuación de observación :
$$z_{}=H_{}{X}_{}+v_{}$$
donde $H_{}$es el modelo de observación, que asigna el espacio de estado real al espacio de observación, $v_{}$es el ruido de observación, su media es cero, y la matriz de covarianza es $R_{}$Y obedecer la distribución normal.
$$v_{}\sim N(0,R_{})$$
donde$cov\left\{v\right(k)\}=R\left(k\right)$ representa la matriz de covarianza.

​ Estado inicial y ruido en cada momento $\{x_{},w_{},\dots ,w_{},v_{},\dots ,v_{}\}$ se consideranindependientesentre sí.

lógica algorítmica

El filtrado de Kalman es una estimación recursiva , es decir, el valor estimado del estado actual se puede calcular siempre que se conozca el valor estimado del estado en el momento anterior y el valor observado del estado actual, por lo que no es necesario registrar información histórica de observaciones o estimaciones. El filtro de Kalman se diferencia de la mayoría de los filtros en que es un filtro de dominio de tiempo puro . No es necesario diseñarlo en el dominio de frecuencia y luego convertirlo al dominio de tiempo como de dominio de frecuenciay otroslos filtros de paso bajo

El filtrado de Kalman se divide principalmente en dos partes: predicción y actualización Necesita construir la ecuación de estado y la ecuación de observación del sistema, y ​​se conoce el estado inicial del sistema. En la fase de predicción, el filtro utiliza estimaciones del estado anterior para hacer una estimación del estado actual. En la fase de actualización, el filtro optimiza la predicción obtenida en la fase de predicción con observaciones del estado actual para obtener una nueva estimación más precisa.

El estado del filtro de Kalman está representado por las siguientes dos variables:

• ${\hat{X}}_{k|}=mi(X_{}|Y_{},Y_{},\dots ,Y_{})$ representa la estimación del estado en el tiempo k.
• ${\hat{X}}_{k|k-}=mi(X_{k-}|Y_{},Y_{},\dots ,Y_{k-})$ representa la predicción del estado en el tiempo k+1 dado el estado de los k momentos pasados.
• ${\widehat{PAG}}_{k|}$, la matriz de covarianza del error de estimación posterior, que mide la precisión del valor estimado.

Entonces el proceso de predicción y actualización (Predicción-Corrección) es el siguiente:
$${\hat{X}}_{k-1|k-}\stackrel{PREDICCIÓN\_\_\_\_\_\_\_\_}{\to }{\hat{X}}_{k|k-}\stackrel{Corrección\_\_\_\_}{\to }{\hat{X}}_{k|}$$

1. Predecir ${\hat{X}}_{k-1|k-}\Rightarrow {\hat{X}}_{k|k-}$

En el paso de predicción, el estado en el momento actual se predice en función del estado y la cantidad de control en el momento anterior. Este valor predicho es una estimación porque no tiene en cuenta los valores observados en el momento actual. La matriz de covarianza de error del valor predicho se calcula a partir de la matriz de covarianza de error y la matriz de covarianza de ruido del sistema en el momento anterior.
{ X ^ k ∣ k − 1 = F kx ^ k − 1 + segundo kuk PAGS k ∣ k − 1 = F k PAGS k − 1 ∣ k − 1 F k T + Q k \begin{casos} \hat{x }_{k|k-1}=F_{k}\hat{x}_{k-1}+B_ku_{k}\\ \\ P_{k|k-1}=F_{k}P_{k -1|k-1}F_k^T+Q_k \end{casos}⎩ ⎨ ⎧X^k ∣ k − 1=FkX^k - 1+BktukPAGk ∣ k − 1=FkPAGk − 1∣ k − 1FkT+qk​

2. 更新${\hat{X}}_{norte|norte-}\Rightarrow {\hat{X}}_{norte|}$

En el paso de actualización, el valor estimado del estado en el momento actual se calcula de acuerdo con el valor observado y el valor pronosticado en el momento actual. Esta estimación es una estimación más precisa porque ya tiene en cuenta las observaciones en el momento actual. La matriz de covarianza de error de la estimación de estado se calcula a partir de la matriz de covarianza de error, la matriz de covarianza de ruido de observación y la ganancia de Kalman calculada en el paso de predicción.
{ K k = PAGS k ∣ k - 1 H k T ( H k PAGS k ∣ k - 1 H k T + R k ) - 1 X ^ k ∣ k = X ^ k ∣ k - 1 + K k ( zk - H kx ^ k ∣ k − 1 ) PAGS k ∣ k = ( yo − K k H k ) PAGS k ∣ k − 1 \begin{cases} K_{k}=P_{k|k-1}H^T_{ k}{(H_{k}P_{k|k-1}H^T_{k}+R_{k})^{-1}}\\ \\ \hat{x}_{k|k}= \hat{x}_{k|k-1}+K_{k}(z_k-H_{k}\hat{x}_{k|k-1})\\ \\ P_{k|k}= (I-K_{k}H_{k})P_{k|k-1} \end{casos}⎩ ⎨ ⎧kk=PAGk ∣ k − 1HkT( HkPAGk ∣ k − 1HkT+Rk)− 1X^k ∣ k=X^k ∣ k − 1+kk( zk−HkX^k ∣ k − 1)PAGk ∣ k=( yo−kkHk) pagk ∣ k − 1​
Las cinco fórmulas anteriores son las fórmulas básicas del filtrado de Kalman. La forma más concisa y fácil de entender del paso de actualización es calcular primero las siguientes tres cantidades:
{ y ^ k = zk − H kx ^ k ∣ k − 1 (medida residual) S k = H k P k ∣ k H k T + R k (covarianza residual de medición) K k = P k ∣ k − 1 H k TS k − 1 (ganancia óptima de Kalman) \begin{casos} \hat{y}_{k}=z_{k } -H_k\hat{x}_{k|k-1}\quad(medida residual)\\ \\ S_{k}=H_{k}P_{k|k}H^T_{k}+R_{ k}\quad(covarianza residual de medición)\\ \\ K_{k}=P_{k|k-1}H^T_{k}{S_{k}^{-1}}\quad(ganancia de Kalman óptima) \end{casos}⎩ ⎨ ⎧y^k=zk−HkX^k ∣ k − 1( medida residual )Sk=HkPAGk ∣ kHkT+Rk( Covarianza residual de medida )kk=PAGk ∣ k − 1HkTSk− 1( Ganancia óptima de Kalman )
Estos se utilizan luego para actualizar las variables de filtro:
{ x ^ k ∣ k = x ^ k ∣ k − 1 + K ky ^ k (estimación de estado actualizada) P k ∣ k = ( I − K k H k ) P k ∣ k − 1 (estimación de covarianza actualizada) \begin{cases} \hat{x}_{k|k}=\hat{x}_{k|k-1}+K_{k}\hat{y }_{ k}\quad(estimación de estado actualizada)\\ \\ P_{k|k}=(I-K_{k}H_{k})P_{k|k-1}\quad(estimación de varianza de asociación actualizada) \end {casos}⎩ ⎨ ⎧X^k ∣ k=X^k ∣ k − 1+kky^k( estimación actualizada del estado )PAGk ∣ k=( yo−kkHk) pagk ∣ k − 1( estimación de covarianza actualizada )
Usa la fórmula anterior para calcular ${PAG}_{k|}$Válido solo en la ganancia óptima de Kalman. Para otras ganancias, la fórmula es más complicada, consulte la derivación .

3. Proceso de filtrado de Kalman

Derivación de fórmula

1. Definición de símbolo

Indique la ecuación
$$X_{}=F_{}{X}_{k-}+B_{}{tu}_{}+w_{}$$
Entre ellos ${tu}_{}$es la señal de entrada, $X_{k-}$es la variable de estado, $w_{}$Indica el ruido del proceso. donde $cov\left\{w\right(k)\}=Q\left(k\right)$。

Ecuación de observación
$$z_{}=H_{}{X}_{}+v_{}$$
donde $z_{}$es la variable observada, $w_{}$representa el ruido de observación. donde $cpagv\left\{w\right(k)\}=R\left(k\right)$。

Estado error de estimación
$${\tilde{X}}_{k|k-}=X_{}-{\hat{X}}_{k|k-}$$
donde ${\tilde{X}}_{k|k-}$Indica que el estimado $k-1$ momento para$X_{}$estimar. En el tiempo k-1, cualquier valor en el tiempo k solo se puede estimar (prediciendo valores futuros) .

Estado de la ecuación de estimación
$${\hat{X}}_{k|k-}=F_{}{\hat{X}}_{k-}+B_{}{tu}_{}$$
Error de estimación de la observación
$${\widetilde{mi}}_{}=z_{}-{\hat{z}}_{k|k-}$$
Ecuación de estimación de observación
$${\hat{z}}_{k|k-}=H_{}{\hat{X}}_{k|k-}$$
Con base en las fórmulas anteriores, se pueden definir dos matrices de covarianza de error importantes:

Matriz de covarianza del error de estado
$${PAG}_{k|k-}=cov\left\{{\tilde{X}}_{k|k-}\right\}$$

Matriz de covarianza del error de observación
$$S_{}=cov\left\{{\widetilde{mi}}_{}\right\}$$
El propósito del filtrado de Kalman es obtener unaexpresión de estimación iterativa que pueda corregirse continuamente en función de los errores, y su forma específica debe ser la siguiente:
$${\hat{X}}_{k|}={\hat{X}}_{k|k-}+k_{}{\widetilde{mi}}_{}$$
Es decir, el valor previsto se corrige en función del error. Aquí $k_{}$Es la ganancia de Kalman. Por lo tanto, necesitamos derivar una expresión iterativa para las variables utilizadas y encontrar la ganancia de Kalman.

2. Derivación

Derivación de la matriz de covarianza del error

Matriz de covarianza de error de estado :

$$\begin{array}{rl}{PAG}_{k|k-}& =cov\{x_{}-{\hat{X}}_{k|k-}\}\\ & =cov\{F_{}{X}_{k-}+B_{}{tu}_{}+w_{}-F_{}{\hat{X}}_{k-}-B_{}{tu}_{}\}\\ & =cov\left\{F_{}\right(X_{k-}-{\hat{X}}_{k-})+w_{}\}\\ & =cov\left\{F_{}\right(X_{k-}-{\hat{X}}_{k-})\}+cov\left\{w_{}\right\}\\ & =F_{}cov\{x_{k-}-{\hat{X}}_{k-}\}{F}_k^{}+q_{}\\ & =F_{}{PAG}_{k|}{F}_k^{}+q_{}\end{array}$$
Aquí obtenemos ${PAG}_{k|}$a ${PAG}_{k|k-}$Una forma recursiva de ${PAG}_{k|}$La forma de notación es para la conveniencia de la derivación posterior, es decir, para obtener
$$\begin{array}{r}{PAG}_{k-1|k-}\to {PAG}_{k|k-}\to {PAG}_{k|}\end{array}$$
De manera similar, se puede obtener la forma recursiva de la matriz de covarianza del error de observación
$$\begin{array}{r}{S}_{}=H_{}{PAG}_{k|k-}{H}_k^{}+R_{}\end{array}$$
Se puede encontrar que $S_{}$y $S_{k-}$No hay conexión directa, por lo que $S_{}$Escrito como $S_{k|k-}$forma.

Por lo tanto, todavía falta ${PAG}_{k|k-}\to {PAG}_{k|}$recursivo

Derivación de la matriz de covarianza posterior

Por definición, la covarianza del error ${PAG}_{k|}$La definición de es la siguiente, simplemente sustituya la definición anterior para deducir y simplificar línea por línea.
$$\begin{array}{rl}{PAG}_{k|}& =cov\left\{{\tilde{X}}_{k|}\right\}\\ & =cov\{x_{}-{\hat{X}}_{k|}\}\\ & =cov\{x_{}-({\hat{X}}_{k|k-}+k_{}{\widetilde{mi}}_{})\}\\ & =cov\{x_{}-({\hat{X}}_{k|k-}+k_{}(z_{}-{\hat{z}}_{k|k-}))\}\\ & =cov\{x_{}-({\hat{X}}_{k|k-}+k_{}(z_{}-H_{}{\hat{X}}_{k|k-}))\}\\ & =cov\{x_{}-({\hat{X}}_{k|k-}+k_{}(H_{}{X}_{}+v_{}-H_{}{\hat{X}}_{k|k-}))\}\\ & =cov\{(x_{}-{\hat{X}}_{k|k-})-k_{}{H}_{}(X_{}-{\hat{X}}_{k|k-})-k_{}{v}_{}\}\\ & =cov\{(yo-k_{}{H}_{}\left)\right(X_{}-{\hat{X}}_{k|k-})-k_{}{v}_{}\}\\ & =cov\{(yo-k_{}{H}_{}\left)\right(X_{}-{\hat{X}}_{k|k-})\}+cov\left\{K_{}{v}_{}\right\}\\ & =(yo-k_{}{H}_{})cov\{(x_{}-{\hat{X}}_{k|k-})\}(yo-k_{}{H}_{}{)}^T+k_{}cov\{v\_{}_{}\}{k}_k^{}\\ & =(yo-k_{}{H}_{}){pag}_{k|k-}(yo-k_{}{H}_{}{)}^T+k_{}{R}_{}{k}_k^{}\end{array}$$
Entre ellos, el error de medida $v_{}$Irrelevante para otros elementos, por lo que puede plantearse por separado. Esta fórmula para cualquier ganancia de Kalman $k_{}$Ambos valen, si $k_{}$es la ganancia de Kalman óptima, entonces se puede simplificar aún más.

Derivación de la ganancia de Kalman óptima

El filtro de Kalman es un estimador del error cuadrático medio mínimo, y la estimación del error de estado posterior es $X_{}-{\hat{X}}_{k|}$

Minimice el valor esperado $mi[|X_{}-{\hat{X}}_{k|}{|}^2]$, lo que equivale a minimizar la matriz de covarianza estimada posterior${PAG}_{k|}$rastro. Expanda y compense los términos en la ecuación anterior:
$$\begin{array}{rl}{PAG}_{k|}& ={PAG}_{k|k-}-k_{}{H}_{}{PAG}_{k|k-}-{PAG}_{k|k-}{H}_k^{}{k}_k^{}+k_{}{H}_{}{PAG}_{k|k-}{H}_k^{}{k}_k^{}+k_{}{R}_{}{k}_k^{}\\ & ={PAG}_{k|k-}-k_{}{H}_{}{PAG}_{k|k-}-{PAG}_{k|k-}{H}_k^{}{k}_k^{}+k_{}(H_{}{PAG}_{k|k-}{H}_k^{}+R_{})k_k^{}\\ & ={PAG}_{k|k-}-k_{}{H}_{}{PAG}_{k|k-}-{PAG}_{k|k-}{H}_k^{}{k}_k^{}+k_{}{S}_{}{k}_k^{}\end{array}$$
Cuando la matriz derivada es 0, obtenga ${PAG}_{k|}$El valor mínimo de la traza:
$$\begin{array}{r}\frac{dtr(P_{k|}}{dk{\_}_{}}=-2(H_{}{PAG}_{k|k-}{)}^T+2K{\_}_{}{S}_{}=\end{array}$$
Aquí se debe usar una fórmula de uso común, como sigue: [Referencia 1] [Referencia 2] [Referencia 3]
$$\begin{array}{r}\frac{dtr(BAC}{dA}=B^{TC}{\_}^{}\end{array}$$
Entonces la ganancia de Kalman se puede resolver:
$$\begin{array}{r}{k}_{}={PAG}_{k|k-}{H}_k^{}{S}_k^{-}\end{array}$$
Esta ganancia se denomina ganancia de Kalman óptima y, cuando se utiliza, da como resultado el error cuadrático medio mínimo.

Simplificación de la fórmula de covarianza de error posterior

Multiplique S k K k T S_kK^T_k en ambos lados de la fórmula de ganancia óptima de Kalman$S_{}{k}_k^{}$得到
$$\begin{array}{r}{k}_{}{S}_{}{k}_k^{}={PAG}_{k|k-}{H}_k^{}{k}_k^{}\end{array}$$
De acuerdo con la fórmula de expansión de la covarianza del error posterior, existe
$$\begin{array}{rl}{PAG}_{k|}& ={PAG}_{k|k-}-k_{}{H}_{}{PAG}_{k|k-}-{PAG}_{k|k-}{H}_k^{}{k}_k^{}+k_{}{S}_{}{k}_k^{}\\ & ={PAG}_{k|k-}-k_{}{H}_{}{PAG}_{k|k-}\end{array}$$
En la práctica, esta fórmula se usa a menudo debido a su cálculo simple, pero debe tenerse en cuenta que esta fórmula solo es válida cuando se usa la ganancia de Kalman óptima. Si la precisión aritmética es siempre tan baja que la estabilidad numérica es un problema, o si se usan deliberadamente ganancias de Kalman subóptimas, entonces no se puede usar esta simplificación; se debe usar la fórmula de covarianza del error a posteriori derivada anteriormente.

referencias

Una introducción al filtro de Kalman

Probablemente el filtro de Kalman más claramente explicado

【EKF】Principio de filtro de Kalman

Enciclopedia Wiki Filtro Kalman

La derivación de fórmula más completa del filtro de Kalman

El rastro de la matriz y la prueba de propiedades relacionadas

Rastro - Wikipedia

[Aprendizaje automático] Resumen y explicación detallada: la traza de la matriz y la derivación de la traza a la matriz