// KalmanCorrectd is called for correcting the predict of the pre-state(first update for every iteration)
public void KalmanCorrectd(Matrixd measurement)
{
// update the output vector(measurement result)
if (measurement.rows != zd.rows || measurement.columns != zd.columns)
{
throw new ArgumentException("Can not update the measurement with different rows or columns.", nameof(measurement));
}
else
{
zd = measurement;
}
// compute the kalman gain and correct the discrete state and estimate uncertainty
Kd = Matrixd.Mul(Matrixd.Mul(Pd, Matrixd.Transpose(Hd)), Matrixd.Inverse(Matrixd.Mul(Matrixd.Mul(Hd, Pd), Matrixd.Transpose(Hd)) + Rd));
xd = xd + Matrixd.Mul(Kd, (zd - Matrixd.Mul(Hd, xd)));
Pd = Matrixd.Mul(Matrixd.Mul((Matrixd.Identity(xd.rows, xd.rows) - Matrixd.Mul(Kd, Hd)), Pd), Matrixd.Transpose(Matrixd.Identity(xd.rows, xd.rows) - Matrixd.Mul(Kd, Hd))) + Matrixd.Mul(Matrixd.Mul(Kd, Rd), Matrixd.Transpose(Kd));
}
protected override void Draw(GameTime gameTime)
{
RenderContext renderContext = new RenderContext(GraphicsDevice, Matrixd.Identity(), 0, 0, 0, 0, 0, RenderContext.LayerPass.TREE_DENSITY_PASS);
foreach (var component in zComponents)
{
component.InitDraw(renderContext);
}
GraphicsDevice.SetRenderTargets(TREE_DENSITY_BUFFER);
DrawAllComponents(renderContext, gameTime);
renderContext.layerPass = RenderContext.LayerPass.GRASS_DENSITY_PASS;
GraphicsDevice.SetRenderTargets(GRASS_DENSITY_BUFFER);
DrawAllComponents(renderContext, gameTime);
GraphicsDevice.BlendState = DEFERRED_RENDERING ? BlendState.Opaque : BlendState.AlphaBlend;
renderContext.layerPass = RenderContext.LayerPass.MAIN_PASS;
GraphicsDevice.SetRenderTargets(DEFERRED_RENDERING ? G_BUFFER : RENDER_BUFFER);
DrawAllComponents(renderContext, gameTime);
GraphicsDevice.BlendState = BlendState.AlphaBlend;
if (DEFERRED_RENDERING)
{
GraphicsDevice.SetRenderTargets(RENDER_BUFFER);
}
DoComposite();
// draw UI directly to backbuffer?
renderContext.layerPass = RenderContext.LayerPass.UI_PASS;
DrawAllComponents(renderContext, gameTime);
if (RECORDING)
{
OSMSectorLoader.SuperSave((Texture2D)RENDER_BUFFER[0].RenderTarget, Path.Combine(RECORD_PATH, $"frame{recordFrame}.png"));
recordFrame++;
}
}
