// 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++; } }