I have a new, challenging programming assignment for you that will take you a while,
but I would like you to implement a multidimensional Kalman filter for the example
I've just given you.
The matrix class is a class for manipulating matrices that should make it really easy.
It has a function that initializes matrices--I'll show you an example in a second.
It can set them down to 0.
It can compute an identity matrix.
It can print out a matrix with show.
It overloads operators like addition, subtraction,
multiplication, and even computes the transpose of a matrix,
and in some more extended code, it can actually invert a matrix
using Cholesky factorization.
There's a function here called inverse.
This matrix class is available.
It's a small version of what is found in typical libraries.
I want to demonstrate it for you just for a second.
You can make a matrix with a command like this with the argument in the parenthesis.
It's a 2-dimensional matrix.
In this case it's a vertical vector.
With the show command, you can print out the result of the vertical vector.
You can put the transpose as follows.
If you run this, you'll find the horizontal vector.
Say you wish to multiply a matrix by a vector,
then we can make a 2 x 2 matrix with this initialization over here,
a matrix of [12., 8.] and [6., 2.].
We can print this matrix.
Here it is: 12, 8, 6, 2.
These are these values over here.
And it can multiply F and a.
So here b = F * a.
And if we show the result, we get the vector 200 and 80.
That's obtained by 10 x 12 + 10 x 8 is 200.
10 x 6 + 10 x 2 is 80.
So using my matrix libraries, I set an initial state.
This is a tracking in 1D where the state is the position
and the velocity.
I initialized both with 0 because I don't know the actual location and the velocity.
I give an uncertainty matrix
where I have a really high uncertainty in the position
and a really high uncertainty in the velocity,
and they're both uncorrelated.
That's the matrix of 1000, 0, 0, 1000.
I specify an external motion, but it's 0, 0, so it has no effect,
so just ignore this.
I build the next state function, which is the one we just discussed,
[1., 1] [0, 1.].
That assumes that the velocity is just being added to the position,
and the velocity and expectation stays the same.
I build a measurement function that extracts just the first
of the 2 values, 1 and 0,
so I can observe the location but not the velocity.
I have a measurement uncertainty.
It happens to be 1 in this example.
And I have an identity matrix, [1., 0.] [0., 1.].
And then I run a filter with these 3 measurements over here,
and what should come out is that by running the filter,
I can estimate the velocity
and therefore make better predictions.
In the filter that I want you to program,
I want the measurement update first and then the motion update.
Every time we run the filter,
I want you to update the measurement first, then the motion.
Here is my empty procedure filter that we have to fill in
where I go through our measurements,
and it builds the measurement update and then the motion update,
the prediction, and then I just print out the resulting estimates.
I do this a number of times, 3 times in this case.
Once we fill this in and I hit the Run button,
I get the following output.
After my first measurement update,
I observed the location 1, which gets copied over
essentially to .99 over here.
I learn nothing about the velocity, so it's still 0, just the way I initialized it.
And then there's an updated uncertainty matrix
which now shows what happens to be a strong correlation,
1000, 1000, 1000, 1000.
That differs from the initial one only that we filled in
the off-diagonal elements.
It happens to be exactly what the Kalman filter does.
And then I'll observe again the 2.
I want the output to now tell me that my next prediction is 3.
It's the observation plus the prediction.
But now I have a really good estimate
on what the velocity is.
It's essentially 1, and the reason is
my Kalman filter was able to use
the Kalman filter equations to find this value.
There's a new covariance matrix,
and for the third observation followed by the prediction,
the prediction is correctly effectively 4, 3.999.
The velocity estimate is correctly 1, which is .99999,
and I have yet another uncertainty matrix which illustrates
that I have a high certainty in the velocity estimate.
Notice a relatively high certainty in the position estimate
compared to my initial uncertainties.
So can you write the the algorithm: 'filter()'
that outputs those exact values over here?
This is an involved programming assignment.
What you have to do is you have to essentially
implement the equations I gave you.
You have to familiarize yourself with the matrix class
and then go and fill in the filter() code
in accordance to the things that I showed you
for the multivariate Kalman filter.
Tengo un nuevo reto de programación para usted que le llevará un tiempo,
me gustaría que implementara un filtro de Kalman multidimensional para el ejemplo
que le acabo de dar.
La clase matrix es una clase para la manipulación de matrices que hará que le sea muy fácil.
Tiene una función que inicializa matrices. Te voy a mostrar un ejemplo en un segundo.
Se puede ajustar a 0.
Se puede calcular una matriz de identidad.
Puede imprimir una matriz para mostrarla.
Sobrecarga de operadores como la suma, la resta,
la multiplicación, e incluso calcula la transpuesta de una matriz,
y en algo más de código, puede invertir una matriz
usando factorización de Cholesky.
Aquí hay una función llamada inverse.
Esta clase matriz está disponible.
Se trata de una versión reducida de lo que se encuentra en las librerias típicas.
Y quiero demostrarselo en sólo un segundo.
Usted puede hacer una matriz con un comando como este con el argumento entre paréntesis.
Es una matriz de 2 dimensiones.
En este caso se trata de un vector vertical.
Y con el comando show, puede imprimir el resultado del vector vertical.
Puede calcular la transpuesta de la siguiente manera.
Si ejecutas esto, verás el vector horizontal.
Y decir que si desea multiplicar una matriz por un vector,
entonces podemos hacer una matriz 2 x 2 con esta inicialización de aquí,
una matriz de [12., 8.] y [6., 2.].
Podemos imprimir esta matriz.
Aquí esta: [12, 8, 6, 2].
Estos son los valores de aquí.
Y podemos multiplicar F y a.
Así que aquí b = F * a.
Y si mostramos el resultado, obtenemos el vector 200 80.
Eso se obtiene 10 * 12 + 10 * 8 es 200.
10 * 6 + 10 * 2 es 80.
Así que usando mi libreria matriz, puse un estado inicial.
Se trata de un sistema de seguimiento en 1D, donde el estado es la posición
y la velocidad.
Yo he inicializado ambos a 0 porque no sé la ubicación real y ni la velocidad.
Le doy una matriz de incertidumbre
donde tengo una incertidumbre muy alta en la posición
y una incertidumbre muy alta en la velocidad,
y ambas están correlacionados.
Esa es la matriz de 1000, 0, 0, 1000.
Puedo especificar un movimiento externo, pero es 0, 0, por lo que no tiene ningún efecto,
así que ignore esto.
Puedo construir la siguiente función de estado, que es la que acabamos de
estudiar, [[1., 1] [0, 1].].
Que supone que la velocidad está siendo sumada a la posición,
y la velocidad sigue siendo la mismo.
Puedo construir una función de medición que extrae sólo el primero
de 2 valores, 1 y 0,
por lo que puedo observar la ubicación, pero no la velocidad.
Tengo una incertidumbre de medición.
que pasa a ser 1 en este ejemplo.
Y tengo una matriz de identidad, [[1., 0.] [0., 1.]].
Y luego ejecuto el filtro con estas 3 medidas de aquí,
y lo que debe salir es que al ejecutar el filtro,
puedo estimar la velocidad
y por lo tanto tomar mejores predicciones.
En el filtro que quiero que programe,
Quiero primero la actualización de la medición, y después la actualización del movimiento.
Así que cada vez que ejecute el filtro,
Quiero que primero se actualice la medición y después el movimiento.
Este es el procedimiento de mi filtro que está vacio y que hay que rellenar
donde voy recorriendo todas las medidas,
y luego construyo la actualización de la medida y la actualización de movimiento,
la predicción, y luego imprimo las estimaciones resultantes.
Puedo hacer esto varias veces, 3 veces en este caso.
Una vez que he rellenado esto y pulso el botón Run,
Me da la siguiente salida.
Después de la actualización de la primera medición,
Observé la posición 1, la cual aparece copiada aquí
esencialmente es 0.999 por aquí.
No me entero de nada de la velocidad, por lo que sigue siendo 0, tal y como yo lo inicialice.
Y luego hay una matriz de incertidumbre actualizada
que ahora muestra lo que sucede hay una correlación fuerte,
1000, 1000, 1000, 1000.
Que difiere de la inicial sólo en los elementos que habíamos
rellenado fuera de la diagonal.
Sucede exactamente lo que hace el filtro de Kalman.
Y luego observo de nuevo el 2.
Quiero que la salida ahora me diga que mi predicción siguiente es: 3.
Es la observación más la predicción.
Pero ahora tengo una estimación muy buena
de lo que es la velocidad.
Básicamente es 1, y la razón es que
mi filtro de Kalman fue capaz de utilizar
las ecuaciones de filtro de Kalman para encontrar este valor.
Hay una nueva matriz de covarianza,
y para la tercera observación siguiendo con la predicción,
la predicción es eficazmente correcta 4, 3.999.
La velocidad estimada 1 es correta, que es 0.99999,
y todavía tengo otra matriz que ilustra la incertidumbre
donde tengo una alta certeza en la estimación de la velocidad,
y también una seguridad relativamente alta en la estimación de la posición
en comparación con mis dudas iniciales.
Así que, ¿puedes escribir el algoritmo de filtro
que da salida a los valores exactos de aquí?
Esta es una tarea de programación complicada.
Lo que tienes que hacer esencialmente es
aplicar las ecuaciones que te di.
Tienes que familiarizarte con la clase matrix
y luego ir a rellenar el código del filtro
de acuerdo a las cosas que te mostré
para el filtro de Kalman multivariante.
少し難しいプログラミングの課題を用意しました
時間がかかると思いますが
多次元のカルマンフィルタを実装してみてください
行列クラスは行列を操作する定義なので
プログラミングを簡単にしてくれるはずです
さらに行列を初期化する関数を持っています
すぐに例をお見せします
値を0にセットするzero
単位行列を計算するidentity
行列を出力するshow
足し算のadd、引き算のsub、
掛け算のmulなどの演算関数に加え
行列を転置させるtransposeを定義します
拡張されたコードではコレスキー因数分解を用いて
行列を実際に反転させます
ここにinverseという関数があります
この行列クラスは便利です
ライブラリでよく見る関数の短いバージョンです
少し説明しましょう
このように括弧の中に引数を持つコマンドで
行列を作れます
二次元行列と言います
この場合は垂直ベクトルです
showコマンドで
垂直ベクトルの結果を出力できます
次のようにtransposeコマンドを実現します
これを実行すれば水平ベクトルの値が得られます
行列にベクトルを掛けたいとします
この初期設定の行列
[12.,8.]と[6.,2.]
2×2の行列を作れます
この行列を出力するとこうなります
12、8、6、2です
この部分の値です 今度はFとaを掛けましょう
次の式を入れます b=F*a
showで出力するとベクトル200と80を得ます
200は10×12+10×8の解です
そして80は10×6+10×2の解です
そこで行列ライブラリを使い初期状態を設定します
一次元で位置と速度の状態を追跡しています
実際の位置と速度は不明なので
0で両方を初期化しました
不確実性を表す行列があります
位置と速度の不確実性はかなり高くなっています
しかし2つは相関しません
行列は[1000.,0.]、[0.,1000.]です
外部動作を指定しますが[0.]、[0.]なので
効果はなく無視することにします
次の状態関数を作ります
ちょうど話した[1.,1]と[0,1.]です
速度は位置に加算されているだけと仮定するので
速度と期待値は同じままです
最初の2つの値1と0を引っ張ってくる
観測関数を作ります
この関数で位置は観察できますが
速度はできません
観測の不確実性は この例では1になります
単位行列は[1.、0.]、[0.、1.]です
ここで3つの要素を持つmeasurementsを用いて
filterを実行します
filterを実行すると速度を推測することができます
より正確な予測をすることができるのです
次のフィルタ・プログラムを書いてください
まず観測更新 次に動作の更新です
filterを実行する度に
最初に観測 次に動作の順番で更新してください
ここに空のfilterプロシージャがあるので
穴を埋めましょう
まずmeasurementsを調べ
次に観測と動作を更新します
その後 予測させて
導き出された推定値を出力します
この場合は3回同じことを行います
穴を埋めたらRunボタンを押し次の出力を得ます
最初の観測更新のあと
位置1を観察し0.99をここへコピーしました
速度は相変わらず不明で
初期化した時の0のままです
そして更新された不確実性の行列があります
強い相関性を示す
1000、1000、1000、1000です
これは穴埋めした初期の状態から
非対角の要素が異なっています
カルマンフィルタの結果とちょうど同じです
もう一度2を観察します
次の予測の出力は3になります 観察+予測です
今回は速度に関して とてもいい予測があります
1である理由はカルマンフィルタが
この値を求めるために数式を使ったためです
新しい共分散行列があります
予測のあとの3回目の観察です
予測は正確には3.999で事実上の4です
速度の予測は0.99999で約1です
もう1つ不確実性の行列があり
速度の観測において高い確実性を描いています
推定位置において初期の不確実性に比べると
確実性が高くなったと気づけましたか?
ここの正確な値を出力する
filterアルゴリズムを書いてください
これは難しい課題ですが
基本的には説明した数式を実装すればいいのです
まずは行列クラスに慣れてから
フィルタコードを埋めてみましょう
多変数カルマンフィルタで説明したことを
思い出してください